/* @(#)splocext.c	17.1.1.1 (ES0-DMD) 01/25/02 17:56:15 */
/*===========================================================================
  Copyright (C) 1995 European Southern Observatory (ESO)
 
  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., 675 Massachusetss Ave, Cambridge, 
  MA 02139, USA.
 
  Corresponding concerning ESO-MIDAS should be addressed as follows:
	Internet e-mail: midas@eso.org
	Postal address: European Southern Observatory
			Data Management Division 
			Karl-Schwarzschild-Strasse 2
			D 85748 Garching bei Muenchen 
			GERMANY
===========================================================================*/

/* Program : splocext.c
/* Author  : P. Ballester   -     ESO Garching                    */
/* Date    : 08.09.94      Version 1.0                            */
/*                                                                */
/* Purpose :                                                      */
/*                                                                */
/* Row by row detection of local extrema (e.g. for wavelet transforms) */
/*                                                                */
/* Input:                                                         */  
/*             - name of input image            : IN_A            */
/*             - name of output peak detection  : OUT_A           */
/*             - name of output window mask     : OUT_B           */

#include <math.h>
#include <midas_def.h>
#define  ipos2D(col,row,npix) row*npix[0]+col

float    *fvector();
void     free_fvector();

main ()

{

    char   inframe[100], outpeak[100], outzero[100];
    char   outwind[100], outmask[100];
    char   cunit[64], ident[72];
    int    imnin, imnp, imnz, imnw, imnm, imns, imnb, unit, null;
    int    actvals, naxis, npix[2];
    float  *pntrin, *pntrp, *pntrw, *pntrz, *pntrm, *pntrs, *pntrb;
    double start[2], step[2];

    int    sign_ind1, sign_ind2, slope_ind, i, j, x, row;
    int    sign_previous, sign_current, rows[2];
    float  previous, current, next, zero_cross;
    float  max_value, abs_value, kappa;

    SCSPRO ("splocext");

    SCKGETC ("IN_A",   1, 60, &actvals, inframe);
    SCKGETC ("IN_B",   1, 60, &actvals, outpeak);
    SCKGETC ("OUT_A",  1, 60, &actvals, outzero);
    SCKGETC ("OUT_B",  1, 60, &actvals, outwind);
    SCKGETC ("INPUTC", 1, 20, &actvals, outmask);
    SCKRDI  ("INPUTI", 1, 2,  &actvals, rows,   &unit, &null);
    SCKRDR  ("INPUTR", 1, 1,  &actvals, &kappa, &unit, &null);

    strcpy(ident,""), strcpy(cunit,"");

    SCIGET(inframe, D_R4_FORMAT, F_I_MODE, F_IMA_TYPE, 2, 
           &naxis, npix, start, step, ident, cunit, (char **)&pntrin, &imnin);


    if (naxis == 1) npix[1]=1;

    strcpy(ident,"Local extrema detection from: ");
    strcat(ident,inframe);
    SCIPUT(outpeak, D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, naxis, 
           npix, start, step, ident, cunit, (char **)&pntrp, &imnp);

    strcpy(ident,"Zero Crossings from: ");
    strcat(ident,inframe);
    SCIPUT(outzero, D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, naxis, 
           npix, start, step, ident, cunit, (char **)&pntrz, &imnz);

    strcpy(ident,"Windows from: ");
    strcat(ident,inframe);
    SCIPUT(outwind, D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, naxis, 
           npix, start, step, ident, cunit, (char **)&pntrw, &imnw);

    strcpy(ident,"Segmented local extrema from: ");
    strcat(ident,inframe);
    SCIPUT("segment.bdf", D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, naxis, 
           npix, start, step, ident, cunit, (char **)&pntrs, &imns);

    strcpy(ident,"Window fusion buffer from: ");
    strcat(ident,inframe);
    SCIPUT("buffer.bdf", D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, naxis, 
           npix, start, step, ident, cunit, (char **)&pntrb, &imnb);

    strcpy(ident,"Features mask from: ");
    strcat(ident,inframe);
    SCIPUT(outmask, D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, 1,
           npix, start, step, ident, cunit, (char **)&pntrm, &imnm);

    for (row=0; row<npix[1]; row++) {
         for (i=0; i<npix[0]; i++)  {

              if (i>0)         previous = pntrin[ipos2D(i-1,row,npix)];
                               current  = pntrin[ipos2D(i  ,row,npix)];
              if (i<npix[0]-1) next     = pntrin[ipos2D(i+1,row,npix)];

              if (i == 0)          previous = next;
              if (i == npix[0]-1)  next     = previous;

              /* Detection of local extrema */
              slope_ind = compare(current,previous) + compare(current,next);
              if (slope_ind == 2 || slope_ind == -2)
                   pntrp[ipos2D(i,row,npix)] = current;
              else
                  pntrp[ipos2D(i,row,npix)] = 0.;

              /* Detection of zero-crossings */
              zero_cross = 0.;
              if (i>0 && i<npix[0]-1) {
                 sign_previous = (previous > 0.) ? 1 : -1;
                 sign_current  = (current  > 0.) ? 1 : -1;
                 if ((sign_previous + sign_current) == 0) 
                     zero_cross = (float) compare(previous,current);
	       }
              pntrz[ipos2D(i,row,npix)] = zero_cross;

	    }

         /* Segmentation of each row to generate window mask */
         for (i=0; i<npix[0]; i++) {
                pntrm[i] = pntrp[ipos2D(i,row,npix)];
                pntrw[ipos2D(i,row,npix)] = 0.;
                pntrb[ipos2D(i,row,npix)] = 0.;
	      }

         segment(pntrm, npix[0], kappa);

         for (i=0; i<npix[0]; i++) {
             pntrs[ipos2D(i,row,npix)] = pntrm[i];
             pntrw[ipos2D(i,row,npix)] = pntrm[i];
	   }

          /* Generation of window mask */

           for (i=0; i<npix[0]; i++) {
            if (pntrm[i] != 0.) {
              for(j=0; 
                  (i+j)<npix[0] && pntrz[ipos2D(i+j,row,npix)]==0.
                  && (pntrp[ipos2D(i+j,row,npix)]==0. || j==0); 
                  j++)
                  pntrw[ipos2D(i+j,row,npix)] = pntrm[i];
                  pntrw[ipos2D(i+j,row,npix)] = pntrm[i];
              for(j=0; 
                  (i-j)>=0  && pntrz[ipos2D(i-j,row,npix)]==0.
                  && (pntrp[ipos2D(i+j,row,npix)]==0. || j==0); 
                  j++)
                  pntrw[ipos2D(i-j,row,npix)] = pntrm[i];
                  pntrw[ipos2D(i-j,row,npix)] = pntrm[i];
	    }
	 }

    /* Removing artifact features */

         for (i=1; i<npix[0]; i++) {

              sign_ind2 = sign(pntrw[ipos2D((i-1),row,npix)]) - 
                          sign(pntrw[ipos2D((i),  row,npix)]);

              sign_ind2 *= sign((float)sign_ind2);

              if (sign_ind2 == 2) {

                  printf("Artifact at %d\n",i);
                  previous = pntrw[ipos2D((i-1),row,npix)];
                  previous *= sign(previous);
                  next     = pntrw[ipos2D((i),row,npix)];
                  next     *= sign(next);

                  if (previous > next) {
                      for(j=0; 
                         (i+j)<npix[0] && 
                         (pntrw[ipos2D((i+j),row,npix)] != 0. || j==0);
                         j++)
                         pntrw[ipos2D((i+j),row,npix)] = 0.;
                         printf("Erased until %d\n",i+j-1);
		    }
                  else {
                      for(j=1; 
                         (i-j)>=0 && 
                         (pntrw[ipos2D((i-j),row,npix)] != 0. || j==1);
                         j++)
                         pntrw[ipos2D((i-j),row,npix)] = 0.;
                         printf("Erased until %d\n",i-j+1);
		  }}}

    } /* For row = .. */

    /* Multi-resolution scale fusion */
    /* for (row=0; row<npix[1]; row++)
        fusion_scales_down (pntrw, pntrb, pntrz, npix, row); 
     for (i=0; i<npix[0]; i++) pntrm[i] = pntrb[ipos2D(i,(npix[1]-1),npix)]; */

    for (row=npix[1]-1; row>=0; row--)
        fusion_scales_up (pntrw, pntrb, pntrz, npix, row);
     for (i=0; i<npix[0]; i++) pntrm[i] = pntrb[ipos2D(i,0,npix)];

    SCSEPI();
  }

/***********************************************************************/
/* Subroutine: compare(a, b) */

static int compare(a,b)
float a,b;
{
int ret;
ret = (a >= b) ? -1 : 1;
if (a == b)     ret = 0;
return(ret);
}


/***********************************************************************/
/* Subroutine: sign(a) */

static int sign(a)
float a;
{
if (a<0.) return(-1);
if (a>0.) return(1);
if (a==0.) return(0);
}

/*************************************************************************/
/*  Sorting algorithm:      Heapsort method                      */
/*  From: Numerical Recipes, Cambridge University Press, p. 226  */

static int sort(n,ra)

int n;
float ra[];
{
        int l,j,ir,i;
        float rra;

        l=(n >> 1)+1;
        ir=n;
        for (;;) {
                if (l > 1)
                        rra=ra[--l];
                else {
                        rra=ra[ir];
                        ra[ir]=ra[1];
                        if (--ir == 1) {
                                ra[1]=rra;
                                return;
                        }
                }
                i=l;
                j=l << 1;
                while (j <= ir) {
                        if (j < ir && ra[j] < ra[j+1]) ++j;
                        if (rra < ra[j]) {
                                ra[i]=ra[j];
                                j += (i=j);
                        }
                        else j=ir+1;
                }
                ra[i]=rra;
        }
}

/***********************************************************************/
static float median(List, N)

float *List;
int   N;

{
float *Histo, med;
int   pos, i;

pos = (N+1)/2;
Histo = fvector(1, N);
for (i=1; i<=N; i++) Histo[i] = List[i-1];
sort(N, Histo);
med = Histo[pos];
free_fvector(Histo, 1, N);
return(med);

}

/***********************************************************************/
static float mean(List, N)

float *List;
int   N;

{
float av;
int   pos, i;

av = 0.;
for (i=0; i<N; i++) av += List[i];
av /= (float)N;
return(av);

}

/***********************************************************************/
/* Subroutine update_window(pntrh, npixh, pntrwin, npix, pos) */

static int update_window(pntrh, npixh, pntrwin, npix, pos, row)
float *pntrh, *pntrwin;
int   npix[], npixh[], pos, row;
{
int i = pos, sign_ref;
float   peak;

peak = pntrh[ipos2D(pos, row, npixh)]/2.;
sign_ref = compare(peak,pntrh[ipos2D(pos, row, npix)]);

while(i < npix[0] && sign_ref == compare(peak,pntrh[ipos2D(i, row, npixh)]))
          pntrwin[ipos2D(i++, row, npix)] = 1.;
i = pos;
while(i>0 && sign_ref == compare(peak,pntrh[ipos2D(i, row, npixh)]))
          pntrwin[ipos2D(i--, row, npix)] = 1.;
}

/***********************************************************************/
/* Subroutine: Segment(List, N, kappa); */

static int segment(List, N, kappa)
float *List, kappa;
int   N;
{
float m, s, t[2], *dev, median();
int   i, cnt=0, cnt0;

dev = fvector(0, N-1);

/* Ignore zeroed values and copies absolute deviations */
for (i=0; i<N; i++) if (List[i] != 0.) {
   dev[cnt] = List[i]; 
   cnt++;
 }


m   = median(dev, cnt);
for (i=0; i<cnt; i++) dev[i] -= m, dev[i] *= dev[i];
s =  median(dev, cnt);
s = (float) sqrt((double)s);

t[0] = m - kappa*s;
t[1] = m + kappa*s;

cnt0 = cnt;
cnt = 0;
for (i=0; i<N; i++) {
  if (List[i] > t[0] && List[i] < t[1]) List[i] = 0.;
  else cnt++, List[i] -= m;
}

printf("m = %f  s = %f Range [%f, %f] Kept %d out of %d\n",
        m,s,t[0],t[1],cnt,cnt0);

free_fvector(dev, 0, N-1);
}



/***********************************************************************/
static int fusion_scales_down (w, b, z, npix, row)

float *w, *b, *z;
int   npix[], row;

{

    int       i,j, k, small_sign, large_sign, max_value = 0;
    float     small_value, large_value;
    float     D_Scale, Scale;

    if (row == 0) {
           for (i=0; i<npix[0]; i++) 
               b[ipos2D(i, row, npix)] = w[ipos2D(i,row,npix)];
	 }
    else {

    Scale     = pow(2.,(double)(row));
    k         = 4.*Scale;

    for (i=0; i<k; i++) 
         b[ipos2D(i, row, npix)] =  b[ipos2D(i, (row-1), npix)];

    for (i=npix[0]-k; i<npix[0]; i++) 
         b[ipos2D(i, row, npix)] =  b[ipos2D(i, (row-1), npix)];

    for (i=k; i<npix[0]-k; i++) {
          if (w[ipos2D(i, row, npix)] == 0.) {
              max_value=0;
              b[ipos2D(i, row, npix)] = b[ipos2D(i, (row-1), npix)];
	    }
          else {
             if (max_value == 0) {
                  max_value = 1;
                  for (j=0; 
                       (((i+j)<npix[0]) && 
                       (z[ipos2D((i+j),(row),npix)])==0.);
                       j++) {
                     small_value = b[ipos2D((i+j)  , (row-1), npix)];
                     small_value *= (float) sign(small_value);
                     large_value = w[ipos2D((i+j)  , row, npix)];
                     large_value *= (float) sign(large_value);
                     if (small_value > large_value) max_value = -1;
		   }}

              b[ipos2D(i, row, npix)] = 0.;
              if (max_value == -1) 
                 b[ipos2D(i, row, npix)] =  b[ipos2D(i, (row-1), npix)];
              if (max_value == 1) 
                  b[ipos2D(i, row, npix)] =  w[ipos2D(i, row, npix)];

	   }}}
  }
/***********************************************************************/
static int fusion_scales_up   (w, b, z, npix, row)

float *w, *b, *z;
int   npix[], row;

{

    int       i,j, k, small_sign, large_sign, max_value = 0;
    float     small_value, large_value;
    float     D_Scale, Scale;

    if (row == npix[1]-1) {
           for (i=0; i<npix[0]; i++) 
               b[ipos2D(i, row, npix)] = w[ipos2D(i,row,npix)];
	 }
    else {

    Scale     = pow(2.,(double)(row));
    k         = 4.*Scale;

    for (i=0; i<k; i++) 
         b[ipos2D(i, row, npix)] =  b[ipos2D(i, (row+1), npix)];

    for (i=npix[0]-k; i<npix[0]; i++) 
         b[ipos2D(i, row, npix)] =  b[ipos2D(i, (row+1), npix)];

    for (i=k; i<npix[0]-k; i++) {
          if (w[ipos2D(i, (row  ), npix)] == 0.  && 
              b[ipos2D(i, (row+1), npix)] == 0.) {
              max_value=0;
              b[ipos2D(i, row, npix)] = b[ipos2D(i, (row+1), npix)]; 
	    }
          else {
             if (max_value == 0) {
                  max_value = 1;
                  for (j=0;
                       (((i+j)<npix[0]) && 
                       (b[ipos2D((i+j),(row+1),npix)] !=0. || 
                        w[ipos2D((i+j),(row  ),npix)] !=0.));
                       j++) {
                     small_value = w[ipos2D((i+j)  , (row), npix)];
                     small_sign  = sign(small_value);
                     large_value = b[ipos2D((i+j)  , (row+1), npix)];
                     large_sign  = sign(large_value);
                     if (small_value*small_sign > large_value*large_sign &&
                         (small_sign + large_sign) != 0) 
                            max_value = -1;
		   }}

              b[ipos2D(i, row, npix)] = 0.;
              if (max_value == -1) 
                 b[ipos2D(i, row, npix)] =  w[ipos2D(i, (row), npix)];
              if (max_value == 1) 
                  b[ipos2D(i, row, npix)] =  b[ipos2D(i, (row+1), npix)];

	   }}}
  }