/* $Id: casu_filt.c,v 1.2 2015/08/07 13:06:54 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/08/07 13:06:54 $
 * $Revision: 1.2 $
 * $Name:  $
 */

/* Includes */

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

#include <cpl.h>
#include <math.h>
#include <string.h>

#include "casu_utils.h"
#include "casu_filt.h"

/* Structure for holding useful information for the stats routine */

typedef struct {
    float sum;
    float sumw;
    int   naver;
    float nextw;
    float lastw;
    float nextval;
    float lastval;
    short int nextc;
    short int lastc;
} nextlast;

static void docols(float *data, unsigned char *bpm, int nx, int ny, 
                   int filter, int stat);
static void dorows(float *data, unsigned char *bpm, int nx, int ny, 
                   int filter, int stat);
static void wraparound(float *data, unsigned char *bpm, int npts, int nfilt, 
                       int whichstat, float **ybuf, unsigned char **ybbuf, 
                       int *nbuf);
static void medavg(float *array, unsigned char *bpm, int *ipoint, int npix, 
                   int whichstat, int newl, nextlast *nl, float *outval, 
                   unsigned char *outbp);
static void quickie(float *array, unsigned char *iarray, int *iarray2, 
                    int lll, int narray);
static void sortm(float *a1, unsigned char *a2, int *a3, int n);
static void plugholes(float *data, unsigned char *bpm, int nx);


/**@{*/

/*---------------------------------------------------------------------------*/
/**
   \ingroup casu_routines
   \brief Do linear filtering on an input map
  
    \par Name:
        casu_bfilt
    \par Purpose:
        Do a linear filtering operation on an input map
    \par Description:
        An image map and its associated bad pixel mask are given. The
        map is smoothed using linear tophat filters.
    \par Language:
        C
    \param data
        The input data map (overwritten by result).
    \param bpm 
        The input bad pixel mask
    \param nx
        The X dimension of the maps
    \param ny
        The Y dimension of the maps
    \param filt
        The width (in pixels) for the filter
    \param stat
        The statistic to be used. 
        - \b MEANCALC
            Smooth using running means
        - \b MEDIANCALC
            Smooth using running medians
    \param axis
        Value of 1 or 2 tells which axis will be smoothed first
    \par QC headers:
        None
    \par DRS headers:
        None
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

extern void casu_bfilt(float *data, unsigned char *bpm, int nx, int ny, 
                       int filt, int stat, int axis) {

    /* Order the reset correction so that the first smoothing is done
       across the axis of the anomaly */

    if (axis == 1) {
        dorows(data,bpm,nx,ny,filt,stat);
        docols(data,bpm,nx,ny,filt,stat);
    } else {
        docols(data,bpm,nx,ny,filt,stat);
        dorows(data,bpm,nx,ny,filt,stat);
    }

}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        docols
    \par Purpose:
        Filter a map along its columns
    \par Description:
        This routine does a column by column linear filter of a map, taking 
        bad pixels into account
    \par Language:
        C
    \param data
        The input data map (overwritten by result).
    \param bpm
        The input bad pixel mask
    \param nx
        The X dimension of the map
    \param ny
        The Y dimension of the map
    \param filter
        The size in pixels of the smoothing filter
    \param stat
        The statistic to be used. 
        - \b MEANCALC
            Smooth using running means
        - \b MEDIANCALC
            Smooth using running medians
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void docols(float *data, unsigned char *bpm, int nx, int ny, 
                   int filter, int stat) {
    int j,k,indx,nbuf;
    unsigned char *goodval,*bbuf;
    float *dbuf;

    /* Get out of here if you've asked for something stupid */

    if (filter <= 0)
        return;

    /* Get some workspace */

    nbuf = max(nx,ny);
    dbuf = cpl_malloc(nbuf*sizeof(*dbuf));
    bbuf = cpl_malloc(nbuf*sizeof(*bbuf));
    goodval = cpl_malloc(ny*sizeof(*goodval));

    /* Now loop for each column and load a column into the buffer */

    for (k = 0; k < nx; k++) {
        memset((char *)goodval,0,ny);
        for (j = 0; j < ny; j++) {
            indx = j*nx + k;
            dbuf[j] = data[indx];
            bbuf[j] = bpm[indx];
        }

        /* Do the smoothing and plug any holes where there are bad pixels */

        casu_dostat(dbuf,bbuf,goodval,ny,filter,stat);
        plugholes(dbuf,goodval,ny);

        /* Transfer the data back */

        for (j = 0; j < ny; j++) {
            indx = j*nx + k;
            data[indx] = dbuf[j];
        }
    } 

    /* Ditch workspace */

    freespace(dbuf);
    freespace(bbuf);
    freespace(goodval);
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        dorows
    \par Purpose:
        Filter a map along its rows
    \par Description:
        This routine does a row by row linear filter of a map, taking 
        bad pixels into account
    \par Language:
        C
    \param data
        The input data map (overwritten by result).
    \param bpm
        The input bad pixel mask
    \param nx
        The X dimension of the map
    \param ny
        The Y dimension of the map
    \param filter
        The size in pixels of the smoothing filter
    \param stat
        The statistic to be used. 
        - \b MEANCALC
            Smooth using running means
        - \b MEDIANCALC
            Smooth using running medians
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void dorows(float *data, unsigned char *bpm, int nx, int ny, 
                   int filter, int stat) {
    int j,k,indx,nbuf;
    unsigned char *goodval,*bbuf;
    float *dbuf;

    /* Get out of here if you've asked for something stupid */

    if (filter <= 0)
        return;

    /* Get some workspace */

    nbuf = max(nx,ny);
    dbuf = cpl_malloc(nbuf*sizeof(*dbuf));
    bbuf = cpl_malloc(nbuf*sizeof(*bbuf));
    goodval = cpl_malloc(nx*sizeof(*goodval));

    /* Now loop for each row and load a row into the buffer */

    for (k = 0; k < ny; k++) {
        memset((char *)goodval,0,nx);
        for (j = 0; j < nx; j++) {
            indx = k*nx + j;
            dbuf[j] = data[indx];
            bbuf[j] = bpm[indx];
        }

        /* Do the smoothing and plug any holes where there are bad pixels */

        casu_dostat(dbuf,bbuf,goodval,nx,filter,stat);
        plugholes(dbuf,goodval,nx);

        /* Transfer the data back */

        for (j = 0; j < nx; j++) {
            indx = k*nx + j;
            data[indx] = dbuf[j];
        }
    }

    /* Ditch workspace */

    freespace(dbuf);
    freespace(bbuf);
    freespace(goodval);
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        casu_dostat
    \par Purpose:
        Do the actual running mean/median smoothing
    \par Description:
        Do a running mean/median of an array for a given filter size. The
        ends are padded out and any bad pixels are taken into account.
    \par Language:
        C
    \param data
        The input data map (overwritten by result).
    \param bpm
        The input bad pixel mask
    \param goodval
        An output bad pixel mask
    \param npts
        The dimension of the data array
    \param nfilt
        The size in pixels of the smoothing filter
    \param whichstat
        The statistic to be used. 
        - \b MEANCALC
            Smooth using running means
        - \b MEDIANCALC
            Smooth using running medians
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

extern void casu_dostat(float *data, unsigned char *bpm, 
                        unsigned char *goodval, int npts, int nfilt,
                        int whichstat) {
    int nbuf,jl,jh,j,*ipoint,ifree,i;
    unsigned char *ybbuf,*barray,bval;
    float *ybuf,*darray,val;
    nextlast nl;

    /* check to make sure the filter size is odd */

    if ((nfilt/2)*2 == nfilt)
        nfilt++;

    /* Do the wrap around and load the data into an oversized array */

    wraparound(data,bpm,npts,nfilt,whichstat,&ybuf,&ybbuf,&nbuf);

    /* Get some data buffers and initialise them */

    darray = cpl_malloc(nfilt*sizeof(*darray));
    barray = cpl_malloc(nfilt*sizeof(*barray));
    ipoint = cpl_malloc(nfilt*sizeof(*ipoint));
    memmove((char *)darray,(char *)ybuf,nfilt*sizeof(*ybuf));
    memmove((char *)barray,(char *)ybbuf,nfilt*sizeof(*ybbuf));
    for (j = 0; j < nfilt; j++)
        ipoint[j] = j;

    /* Do the stat for the first point */

    ifree = 0;
    medavg(darray,barray,ipoint,nfilt,whichstat,-1,&nl,&val,&bval);
    if (! bval) 
        data[0] = val;
    goodval[0] = bval;

    /* Now do the stats for all subsequent points. The oldest point in the
       buffer is replaced by the next raw point */

    jl = nfilt;
    jh = nfilt + npts - 2;
    for (j = jl; j <= jh; j++) {
        for (i = 0; i < nfilt; i++) {
            if (ipoint[i] == 0) {
                ifree = i;
                ipoint[i] = nfilt - 1;
                nl.lastval = darray[ifree];
                nl.lastw = 0.0;
                nl.lastc = 0;
                if (barray[ifree] == 0) {
                    nl.lastw = 1.0;
                    nl.lastc = 1;
                }                
                darray[ifree] = ybuf[j];
                barray[ifree] = ybbuf[j];
                nl.nextval = darray[ifree];
                nl.nextw = 0.0;
                nl.nextc = 0;
                if (barray[ifree] == 0) {
                    nl.nextw = 1.0;
                    nl.nextc = 1;
                }                
            } else
                ipoint[i]--;
        }
        medavg(darray,barray,ipoint,nfilt,whichstat,ifree,&nl,&val,&bval);
        if (! bval) 
            data[j-jl+1] = val;
        goodval[j-jl+1] = bval;
    }

    /* Ditch workspace */

    freespace(darray);
    freespace(barray);
    freespace(ipoint);
    freespace(ybuf);
    freespace(ybbuf);
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        wraparound
    \par Purpose:
        Extend the data buffer ends in prepration for filtering
    \par Description:
        The ends of a data buffer are extended by doing a wrap around
        interpolation. Size of the extensions on either end are determined
        by the filter size in pixels.
    \par Language:
        C
    \param data
        The input data map (overwritten by result).
    \param bpm
        The input bad pixel mask
    \param npts
        The dimension of the data array
    \param nfilt
        The size in pixels of the smoothing filter
    \param whichstat
        The statistic to be used. 
        - \b MEANCALC
            Smooth using running means
        - \b MEDIANCALC
            Smooth using running medians
    \param ybuf
        The extended data array
    \param ybbuf
        The extended bad pixel mask
    \param nbuf
        The size of the extended arrays
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void wraparound(float *data, unsigned char *bpm, int npts, int nfilt, 
    int whichstat, float **ybuf, unsigned char **ybbuf, int *nbuf) {

    float *darray,xmns,xmnf;
    int i1,ilow,i,*ipoint;
    unsigned char *barray,bxmns,bxmnf;
    nextlast nl;

    /* Do some padding at the edges */

    i1 = nfilt/2;
    ilow = max(3,nfilt/4);
    ilow = (ilow/2)*2 + 1;

    /* Get some workspace */

    darray = cpl_malloc(nfilt*sizeof(*darray));
    barray = cpl_malloc(nfilt*sizeof(*barray));
    ipoint = cpl_calloc(nfilt,sizeof(*ipoint));
    *nbuf = npts + 2*i1;
    *ybuf = cpl_malloc(*nbuf*sizeof(float));
    *ybbuf = cpl_malloc(*nbuf*sizeof(unsigned char));

    /* Do the wrap around.*/

    memmove((char *)darray,(char *)data,ilow*sizeof(*data));
    memmove((char *)barray,(char *)bpm,ilow*sizeof(*bpm));
    medavg(darray,barray,ipoint,ilow,whichstat,-1,&nl,&xmns,&bxmns);
    memmove((char *)darray,(char *)(data+npts-ilow),ilow*sizeof(*data));
    memmove((char *)barray,(char *)(bpm+npts-ilow),ilow*sizeof(*bpm));
    medavg(darray,barray,ipoint,ilow,whichstat,-1,&nl,&xmnf,&bxmnf);
    for (i = 0; i < i1; i++) {
        if (! bxmns) {
            (*ybuf)[i] = 2.0*xmns - data[i1+ilow-i-1];
            (*ybbuf)[i] = bpm[i1+ilow-i-1];
        } else {
            (*ybuf)[i] = data[i1+ilow-i-1];
            (*ybbuf)[i] = 1;
        }
        if (! bxmnf) {
            (*ybuf)[npts+i1+i] = 2.0*xmnf - data[npts-i-ilow-1];
            (*ybbuf)[npts+i1+i] = bpm[npts-i-ilow-1];
        } else {
            (*ybuf)[npts+i1+i] = data[npts-i-ilow-1];
            (*ybbuf)[npts+i1+i] = 1;
        }
    }

    /* Now place the full line into the buffer */

    memmove((char *)(*ybuf+i1),data,npts*sizeof(*data));
    memmove((char *)(*ybbuf+i1),bpm,npts*sizeof(*bpm));

    /* Free workspace */

    freespace(darray);
    freespace(barray);
    freespace(ipoint);
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        medavg
    \par Purpose:
        Do a mean/median of an array
    \par Description:
        Does a mean/median of an array. This was designed to be called 
        repeatedly for the purposes of running statistics. 
    \par Language:
        C
    \param array
        The input data array
    \param bpm
        The input bad pixel mask
    \param ipoint
        An array which gives the sort order of the input points. This should
        be an array of values 1..npix when 'newl' is -1 and this is the first
        of a number of repeated calls. If this call is a one-off, the newl 
        should be set to -1 and this can be an array of zeros.
    \param npix
        The number of pixels in the arrays
    \param whichstat
        The statistic to be used. 
        - \b MEANCALC
            Smooth using running means
        - \b MEDIANCALC
            Smooth using running medians
    \param newl
        The location in the arrays of the newest pixel for the running stat
    \param outval
        The output mean/median
    \param outbp
        The output bad pixel flag
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void medavg(float *array, unsigned char *bpm, int *ipoint, int npix, 
                   int whichstat, int newl, nextlast *nl, float *outval, 
                   unsigned char *outbp) {

    float *buf=NULL;
    int m,i;

    /* Prepare for median. If there is no new element then do a proper 
       sort. Otherwise to a much quicker sort because the input array should
       be almost sorted as it is. */
    
    m = 0;
    if (whichstat == MEDIANCALC) {
        if (newl == -1) 
            sortm(array,bpm,ipoint,npix);
        else  
            quickie(array,bpm,ipoint,newl,npix);

        /* Get some workspace */

        buf = cpl_malloc(npix*sizeof(*buf));

        /* Now put everything that's good in the buffer */

        m = 0;
        for (i = 0; i < npix; i++) {
            if (bpm[i] == 0) {
                buf[m] = array[i];
                m++;
            }
        }

    /* Prepare for a mean. If there are no new values in the input arrays
       then do the summations. If there is, then include it in the summations
       from the previous calls and removed the oldest value from the
       summations */

    } else if (whichstat == MEANCALC) {
        if (newl == -1) {
            nl->sum = 0.0;
            nl->sumw = 0.0;
            nl->naver = 0;
            for (i = 0; i < npix; i++) {
                if (bpm[i] == 0) {
                    nl->sum += array[i];
                    nl->sumw += 1.0;
                    nl->naver += 1;
                }
            }
            m = nl->naver;
        } else {
            nl->sum += (nl->nextw*nl->nextval - nl->lastw*nl->lastval);
            nl->sumw += (nl->nextw - nl->lastw);
            nl->naver += (nl->nextc - nl->lastc);
            m = nl->naver;
        }
    }
        
    /* If they were all bad, then send a null result back */

    if (m == 0) {
        *outval = 0.0;
        *outbp = 1;
        if (whichstat == MEDIANCALC)
            freespace(buf);

    /* Otherwise calculate the relevant stat */

    } else {
        if (whichstat == MEDIANCALC) {
            if (!(m & 1)) 
                *outval = 0.5*(buf[(m/2)-1] + buf[m/2]);
            else
                *outval = buf[m/2];
            freespace(buf);
        } else if (whichstat == MEANCALC)
            *outval = (nl->sum)/(nl->sumw);
        *outbp = 0;
    }
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        quickie
    \par Purpose:
        Do a sort of a nearly sorted array
    \par Description:
        Takes an array that is nearly sorted with the exception of a single
        point. Rearranges the array so that point is the placed in sort order.
    \par Language:
        C
    \param array
        The input data array
    \param iarray
        A cosorted unsigned char array
    \param iarray2
        A cosorted integer array. 
    \param testloc
        The array index for the unsorted value
    \param narray
        The number of elements in each array
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void quickie(float *array, unsigned char *iarray, int *iarray2, 
    int testloc, int narray) {

    float test;
    int i,j,npt,it2;
    unsigned char it;

    test = array[testloc];
    it = iarray[testloc];
    it2 = iarray2[testloc];
    j = -1;
    for (i = 0; i < narray; i++) {
        if (i != testloc && test <= array[i]) {
            j = i;
            break;
        }
    }
    if (j == -1) 
        j = narray;
    if (j - 1 == testloc)
        return;

    if (j - testloc < 0) {
        npt = testloc - j;
        for (i = 0; i < npt; i++) {
            array[testloc-i] = array[testloc-i-1];
            iarray[testloc-i] = iarray[testloc-i-1];
            iarray2[testloc-i] = iarray2[testloc-i-1];
        }
        array[j] = test;
        iarray[j] = it;
        iarray2[j] = it2;
    } else {
        j--;
        npt = j - testloc;
        if (npt != 0) {
            for (i = 0; i < npt; i++) {
                array[testloc+i] = array[testloc+i+1];
                iarray[testloc+i] = iarray[testloc+i+1];
                iarray2[testloc+i] = iarray2[testloc+i+1];
            }
        }
        array[j] = test;
        iarray[j] = it;
        iarray2[j] = it2;
    }
}
            
/*---------------------------------------------------------------------------*/
/**
    \par Name:
        sortm
    \par Purpose:
        Sort an array and cosort two others
    \par Description:
        Modified Shell sort routine. Sorts on a floating array and cosorts
        two others.
    \par Language:
        C
    \param a1
        The input floating point data array on which the sort is to be done
    \param a2
        A cosorted unsigned char array
    \param a3
        A cosorted integer array. 
    \param n
        The number of elements in each array
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void sortm(float *a1, unsigned char *a2, int *a3, int n) {
    int iii,ii,i,ifin,j,b3;
    unsigned char b2;
    float b1;

    iii = 4;
    while (iii < n)
        iii *= 2;
    iii = min(n,(3*iii)/4 - 1);

    while (iii > 1) {
        iii /= 2;
        ifin = n - iii;
        for (ii = 0; ii < ifin; ii++) {
            i = ii;
            j = i + iii;
            if (a1[i] > a1[j]) {
                b1 = a1[j];
                b2 = a2[j];
                b3 = a3[j];
                while (1) {
                    a1[j] = a1[i];
                    a2[j] = a2[i];
                    a3[j] = a3[i];
                    j = i;
                    i = i - iii;
                    if (i < 0 || a1[i] <= b1) 
                        break;
                }
                a1[j] = b1;
                a2[j] = b2;
                a3[j] = b3;
            }
        }
    }
}

/*---------------------------------------------------------------------------*/
/**
    \par Name:
        plugholes
    \par Purpose:
        Replace bad values in an array.
    \par Description:
        Bad values in an array are replaced by a linearly interpolated
        value.
    \par Language:
        C
    \param data
        The input data array (overwritten)
    \param bpm
        The bad pixel mask
    \param nx
        The number of elements in each array
    \author
        Jim Lewis, CASU
 */
/*---------------------------------------------------------------------------*/

static void plugholes(float *data, unsigned char *bpm, int nx) {
    int i,ifirst,ilast,i1,i2,j;
    float nc,d1,d2,t1,t2,slope;

    /* First of all, find the first good value in the array */

    i = 0;
    while (i < nx && bpm[i] != 0)
        i++;
    ifirst = i;

    /* If all the values in the array are bad, then do nothing */

    if (ifirst == nx)
        return;

    /* Find the last good value in the array */

    i = nx - 1;
    while (i >= 0 && bpm[i] != 0) 
        i--;
    ilast = i;

    /* Right, now start from the first good value and fill in any holes in the
       middle part of the array */

    i = ifirst;
    while (i <= ilast) {
        if (bpm[i] == 0) {
            i++;
            continue;
        }
        i1 = i - 1;
        while (bpm[i] != 0) 
            i++;
        i2 = i;
        nc = (float)(i2 - i1 + 1);
        d1 = data[i1];
        d2 = data[i2];
        for (j = i1+1; j <= i2-1; j++) {
            t1 = 1.0 - (float)(j - i1)/nc;
            t2 = 1.0 - t1;
            data[j] = t1*d1 + t2*d2;
        }
    }

    /* Now the left bit... */

    if (ifirst > 0) {
        slope = data[ifirst+1] - data[ifirst];
        for (j = 0; j < ifirst; j++)
            data[j] = slope*(j - ifirst) + data[ifirst];
    }

    /* Now the right bit... */

    if (ilast < nx - 1) {
        slope = data[ilast] - data[ilast-1];
        for (j = ilast; j < nx; j++) 
            data[j] = slope*(j - ilast) + data[ilast];
    }
}

/**@}*/


/* 

$Log: casu_filt.c,v $
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.5  2015/06/09 18:31:56  jim
Made casu_dostat external

Revision 1.4  2015/01/29 11:46:18  jim
modified comments

Revision 1.3  2014/03/26 15:35:26  jim
Modified to remove globals

Revision 1.2  2013/11/21 09:38:13  jim
detabbed

Revision 1.1.1.1  2013-08-27 12:07:48  jim
Imported


*/
