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

/*
 * $Author: jtaylor $
 * $Date: 2012/02/02 10:09:30 $
 * $Revision: 1.18 $
 * $Name: visir-3_5_1 $
 */

#include <math.h>

#define TYPE_ADD(a) CONCAT2X(a, PIXEL_TYPE)
#define TYPE_ADD_CONST(a) CONCAT2X(a, CONCAT2X(PIXEL_TYPE, const))

/* Swap macro */
#define PIXEL_TYPE_SWAP(a, b) \
    { register const PIXEL_TYPE t=(a); (a)=(b); (b)=t; }

static PIXEL_TYPE TYPE_ADD(cpl_tools_get_kth)(PIXEL_TYPE *, int, int);

static
double TYPE_ADD(cpl_tools_get_variancesum)(const PIXEL_TYPE *, int, double *);


/*----------------------------------------------------------------------------*/
/**
  @internal
  @brief    Compute the summed sample variance of an array
  @param    a     The array
  @param    n     The (non-negative) array size
  @param    pmean Iff non-NULL, *pmean is the mean (at no extra cost)
  @return   The summed sample variance, S(n) = sum((a_i-mean)^2) (i=1 -> n)
  @note Even with rounding errors the returned result is always non-negative
  @see CPL: cpl_tools_get_variancesum

 */
/*----------------------------------------------------------------------------*/
double TYPE_ADD(cpl_tools_get_variancesum)(const PIXEL_TYPE * a,
                                          int n, double * pmean)
{
    double varsum = 0.0;
    double mean = 0.0;
    int i;

    cpl_ensure(a != NULL, CPL_ERROR_NULL_INPUT,    0.0);
    cpl_ensure(n >= 0,    CPL_ERROR_ILLEGAL_INPUT, 0.0);

    for (i=0; i < n; i++) {
        const double delta = (double)a[i] - mean;

        varsum += i * delta * delta / (double)(i + 1);
        mean   += delta / (double)(i + 1);
    }

    if (pmean != NULL) *pmean = mean;

    return varsum;
}

/*----------------------------------------------------------------------------*/
/**
  @internal
  @brief  Get the kth smallest value in a numerical array
  @param  self The array to permute and request from
  @param  n    The number of elements in the array
  @param  k    The requested value position in the sorted array, zero for 1st
  @return The kth smallest value in the partially sorted array.
  @note Since the function is not exported its error checking is disabled
  @see CPL: cpl_tools_get_kth

  After a successful call, self is permuted so elements less than the kth have
  lower indices, while elements greather than the kth have higher indices.

 */
/*----------------------------------------------------------------------------*/
static
PIXEL_TYPE TYPE_ADD(cpl_tools_get_kth)(PIXEL_TYPE * self,
                                       int        n,
                                       int        k)
{
    register int l = 0;
    register int m = n - 1;
    register int i = l;
    register int j = m;

    cpl_ensure(self != NULL, CPL_ERROR_NULL_INPUT,          (PIXEL_TYPE)0);
    cpl_ensure(k >= 0,       CPL_ERROR_ILLEGAL_INPUT,       (PIXEL_TYPE)0);
    cpl_ensure(k <  n,       CPL_ERROR_ACCESS_OUT_OF_RANGE, (PIXEL_TYPE)0);

    while (l < m) {
        register const PIXEL_TYPE x = self[k];

        do {
            while (self[i] < x) i++;
            while (x < self[j]) j--;
            if (i <= j) {
                PIXEL_TYPE_SWAP(self[i], self[j]);
                i++; j--;
            }
        } while (i <= j);

        /* assert( j < i ); */

        /* The original implementation has two index comparisons and
           two, three or four index assignments. This has been reduced
           to one or two index comparisons and two index assignments.
        */

        if (k <= j) {
            /* assert( k < i ); */
            m = j;
            i = l;
        } else {
            if (k < i) {
                m = j;
            } else {
                j = m;
            }
            l = i;
        }
    }
    return self[k];
}


/*----------------------------------------------------------------------------*/
/**
  @brief  Use kappa-sigma clipping to reject outlier pixels
  @param  self     The imagelist of the given pixel type to process w. bpm
  @param  devlist  Append stdev maps to this list
  @param  keepfrac The small values initially rejected
  @param  kappa    +/- Kappa * sigma is rejected
  @param  maxite   Maximum number of iterations to perform
  @return CPL_ERROR_NONE iff OK, otherwise the relevant CPL error code.
  @see visir_util_clip_kappa_sigma
 */
/*----------------------------------------------------------------------------*/
static
cpl_error_code TYPE_ADD(visir_util_clip_kappa_sigma)(cpl_imagelist * self,
                                                     cpl_imagelist * devlist,
                                                     double keepfrac,
                                                     double kappa, int maxite,
                                                     const int * shifts)
{
    const int         nz  = cpl_imagelist_get_size(self);
    const cpl_image * img = cpl_imagelist_get_const(self, 0);
    const int         nx = cpl_image_get_size_x(img);
    const int         ny = cpl_image_get_size_y(img);
    /* Reject this many of the minimum (and maximum) values */
    const int         minrej = (int)((double)nz * 0.5 * (1.0 - keepfrac) + 0.5);
    PIXEL_TYPE * pvalues = (PIXEL_TYPE*)cpl_malloc(nz * sizeof(*pvalues));
    /* Pointers to the nz pixel buffers */
    const PIXEL_TYPE ** pimg = cpl_malloc((size_t)nz * sizeof(PIXEL_TYPE *));
    cpl_binary       ** pbpm = cpl_malloc((size_t)nz * sizeof(cpl_binary *));
    cpl_image         * imgstdev0 = cpl_image_new(nx, ny, STDEV_TYPE);
    cpl_image         * imgstdevn = cpl_image_new(nx, ny, STDEV_TYPE);
    int k, i, j;


    bug_if(cpl_image_get_type(img) != PIXEL_TYPE_CPL);

    //error_if(nz < 3, CPL_ERROR_DATA_NOT_FOUND, "nz = %d < 3", nz);

    error_if(keepfrac < 0.0, CPL_ERROR_ILLEGAL_INPUT, "Parameter keepfrac = "
             "%g < 0.0", keepfrac);
    error_if(keepfrac > 1.0, CPL_ERROR_ILLEGAL_INPUT, "Parameter keepfrac = "
             "%g > 1.0", keepfrac);
    error_if(kappa    < 0.0, CPL_ERROR_ILLEGAL_INPUT, "Parameter kappa = "
             "%g < 0.0", kappa);
    error_if(maxite   < 0,   CPL_ERROR_ILLEGAL_INPUT, "Parameter maxite = "
             "%d < 0", maxite);        

    for (k = 0; k < nz; k++) {
        cpl_image  * imgk = cpl_imagelist_get(self, k);
        cpl_mask   * bpm  = cpl_image_get_bpm(imgk);

        pimg[k] = TYPE_ADD_CONST(cpl_image_get_data)(imgk);
        pbpm[k] = cpl_mask_get_data(bpm);

        bug_if(pimg[k] == NULL);
        bug_if(pbpm[k] == NULL);

    }

    for (j = 0; j < ny; j++) {
        for (i = 0; i < nx; i++) {
            int nok = 0;

            for (k = 0; k < nz; k++) {
                const int ii = (i + shifts[k * 2]) +
                               (j + shifts[k * 2 + 1]) * nx;
                if (i + shifts[k * 2] >= nx || i + shifts[k * 2] < 0 ||
                    j + shifts[k * 2 + 1] >= ny || j + shifts[k * 2 + 1] < 0)
                    continue;

                if (!pbpm[k][ii]) {
                    pvalues[nok++] = pimg[k][ii];
                }
            }

            if (nok > 1) { /* Cannot clip with just one value */
                /* Index of 1st value to skip */
                const int ithmin = minrej - (nz - nok) / 2 - 1;
                double median, mean, stdev, varsum;
                PIXEL_TYPE * pmedian = pvalues;
                int nmedian = nok;
                int nite;

                if (0 <= ithmin && 2 * ithmin + 3 <= nok) {
                    /* Need at least 3 values for a proper median + stdev */
                    /* Index of last value to skip */
                    const int ithmax = nok - ithmin - 1;
                    (void)TYPE_ADD(cpl_tools_get_kth)(pvalues, nok, ithmax);
                    (void)TYPE_ADD(cpl_tools_get_kth)(pvalues, ithmax, ithmin);
                    /* Already ignored values are skipped in median */
                    pmedian += ithmin     + 1;
                    nmedian -= ithmin * 2 + 2;
                }
                bug_if( nmedian <= 1 );

                median = (double)TYPE_ADD(cpl_tools_get_kth)(pmedian, nmedian,
                                                             (nmedian - 1) / 2);
                if (!(nmedian & 1)) {
                    /* Even number of samples, use mean of two central values */
                    /* Already did lower half */
                    median  += (double)TYPE_ADD(cpl_tools_get_kth)(pmedian +
                                                                   nmedian / 2,
                                                                   nmedian / 2,
                                                                   0);
                    median  *= 0.5;
                }

                varsum = TYPE_ADD(cpl_tools_get_variancesum)(pmedian, nmedian,
                                                             NULL);
                stdev = sqrt(varsum / (double)(nmedian - 1));

                bug_if(cpl_image_set(imgstdev0, 1 + i, 1 + j, stdev));

                for (nite = 0; nite < maxite; nite++) {
                    /* For nite > 0 use mean of previous iteration */
                    const double center = nite ? mean : median;
                    const double lolim  = center - kappa * stdev;
                    const double hilim  = center + kappa * stdev;
                    const int    prevok = nok;

                    nok    = 0;
                    mean   = 0.0;
                    varsum = 0.0;

                    for (k = 0; k < nz; k++) {
                        const int ii = (i + shifts[k * 2]) +
                                       (j + shifts[k * 2 + 1]) * nx;
                        if (i + shifts[k * 2] >= nx || i + shifts[k * 2] < 0 ||
                            j + shifts[k * 2 + 1] >= ny || j + shifts[k * 2 + 1] < 0) {
                            continue;
                        }
                        /*
                        cpl_msg_debug(cpl_func, "it %d z%d x%d(%d) y%d(%d): "
                                      "%.2f <= %.2f <= %.2f, bad %d", nite, k,
                                      i + shifts[k * 2],
                                      shifts[k * 2], j + shifts[k * 2 + 1],
                                      shifts[k * 2 + 1], lolim,
                                      (double)pimg[k][ii + ishift], hilim,
                                      pbpm[k][ii + ishift]);
                        */

                        if (!pbpm[k][ii]) {
                            if (lolim <= pimg[k][ii] &&
                                pimg[k][ii] <= hilim) {
                                /* Compute also for last iteration, for final
                                   standard deviation */
                                const double delta =
                                    (double)pimg[k][ii] - mean;

                                varsum += nok * delta * delta
                                    / (double)(nok + 1);
                                mean   += delta / (double)(nok + 1);

                                nok++;
                            } else {
                                /* Reject outlier */
                                cpl_msg_debug(cpl_func, "rejected");
                                pbpm[k][ii] = CPL_BINARY_1;
                            }
                        }
                    }

                    if (nok < 2) {
                        stdev = 0.0;
                        break; /* No more values */
                    }
                    if (nok == prevok) break; /* Convergence */
                    stdev = sqrt(varsum / (double)(nok - 1));
                }
                bug_if(cpl_image_set(imgstdevn, 1 + i, 1 + j, stdev));
            }
        }
    }

    bug_if(cpl_imagelist_set(devlist, imgstdev0, 0));
    imgstdev0 = NULL;
    bug_if(cpl_imagelist_set(devlist, imgstdevn, 1));
    imgstdevn = NULL;

    end_skip;

    cpl_image_delete(imgstdev0);
    cpl_image_delete(imgstdevn);
    cpl_free(pvalues);
    cpl_free(pimg);
    cpl_free(pbpm);

    return cpl_error_get_code();
}

#undef PIXEL_TYPE_SWAP
