/* @(#)necfindmax.c	17.1.1.1 (ES0-DMD) 01/25/02 17:51:29 */
/*===========================================================================
  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 : findmax.c                                            */
/* Author  : P. Ballester   -     ESO Garching                    */
/* Date    : 23.03.91      Version 1.0                            */
/*                                                                */
/* Purpose :                                                      */
/*                                                                */
/*    a) Find maximum                                             */
/*    b) Find location of parameters geometrical center           */
/*                                                                */
/* Input:                                                         */  
/*             - name of input image            : IN_A            */
/*             - Center threshold               : INPUTR(1)       */
/*             - Half-width of orders           : INPUTI(1)       */
/*             - Number of col. in orig. image  : INPUTI(2)       */
/*             - Distance between traces        : INPUTI(3)       */
/*             - Number traces                  : INPUTI(4)       */
/*                                                                */
/* Output:                                                        */
/*             - X max                          : OUTPUTR(1)      */
/*             - Y max                          : OUTPUTR(2)      */
/*             - Peak                           : OUTPUTR(3)      */
/*             - Estimated width                : OUTPUTR(4)      */
/*                                                                */

#include <ok.h>
#include <math.h>
#include <string.h>
#include <stdio.h>

#include <midas_def.h>
#include <tbldef.h>

#include <hough.h>
double correlate();

main ()
{
    int    keep_on_working;

    char        frame[TEXT_LEN+1], text[TEXT_LEN], method[TEXT_LEN];
    char        ident[TEXT_LEN+1] , cunit[16*3 + 1], restab[TEXT_LEN];

    int         hw, orgcol, ntrace, widtrace;

    int         imnoa, naxis, npix[2], inpi[4];
    int         null, actvals, kunit;

    float       *pntra;
    float       outr[4], thres;
    float       xmax, ymax, peak=1., xcntr, ycntr, fwhm, fwhm_del;

    float       slope[200], intercept[200], sl, inter, rms, sl_width=0.1;
    int         sl_window;

    double      start[2] , step[2];

    int    tid, res_slope, res_orig, res_peak, res_fwhm;
    int    nord=0, ncol, icol[10], minmax[2];
    float  lowmax=0., cluster;

    SCSPRO ("findmax");

    SCKRDR  ("INPUTR", 1, 1, &actvals, &thres, &kunit, &null);
    SCKRDR  ("INPUTR", 2, 1, &actvals, &cluster, &kunit, &null);
    SCKRDI  ("INPUTI", 1, 4, &actvals, inpi, &kunit, &null);
    SCKRDI  ("INPUTI", 5, 2, &actvals, minmax, &kunit, &null);
    SCKGETC ("INPUTC", 1, 1, &actvals, method);

    hw         = inpi[0];
    orgcol     = inpi[1];
    ntrace     = inpi[3];
    widtrace   = inpi[2];

    SCKGETC ("IN_A", 1, 60, &actvals, frame);
    SCKGETC ("IN_B", 1, 60, &actvals, restab);

    strcpy(ident," ");
    strcpy(cunit," ");
    SCIGET (frame, D_R4_FORMAT, F_IO_MODE, F_IMA_TYPE, 2,
                &naxis, npix, start, step, ident, cunit, (char **)&pntra,
                &imnoa);

    TCTINI (restab, F_TRANS, F_O_MODE, 5, 500, &tid);
    ncol = 0;
    TCCINI (tid, D_R4_FORMAT, 1, "E12.6", "", ":SLOPE", &res_slope);
    icol[ncol++] = res_slope;
    TCCINI (tid, D_R4_FORMAT, 1, "E12.6", "", ":ORIG",  &res_orig);
    icol[ncol++] = res_orig;
    TCCINI (tid, D_R4_FORMAT, 1, "E12.6", "", ":PEAK",  &res_peak);
    icol[ncol++] = res_peak;
    TCCINI (tid, D_R4_FORMAT, 1, "E12.6", "", ":FWHM",  &res_fwhm);
    icol[ncol++] = res_fwhm;

    keep_on_working = TRUE;
    find_max (pntra, npix, &xmax, &ymax, &peak); /* First maximum */
    cor_find_width (pntra, npix, xmax, ymax, peak, &fwhm);
    fwhm -= 1.;
    sl_window = sl_width/step[0];

    while (keep_on_working) {

       find_cntr (pntra, npix, orgcol, xmax, ymax, peak, step, fwhm, thres, 
                                  &xcntr, &ycntr);

       slope[nord] = xcntr, intercept[nord] = ycntr;

       outr[0] = start[0] + (double) xcntr *step[0]; /* Slope */
       outr[1] = start[1] + (double) ycntr *step[1]; /* Origin Intercept */
       outr[2] = peak;                               /* Peak */
       outr[3] = fwhm*step[1];                       /* Fwhm */

       if (nord == 0) {
         if (toupper(method[0]) == 'L')  hw = inpi[0];
         if (toupper(method[0]) == 'M')  hw = outr[3] + 3;
         if (toupper(method[0]) == 'H')  hw = 0;
         lowmax = peak*cluster;
	 }

       if (hw == 0)
          fwhm_del = fwhm;
       else
          fwhm_del = (float) hw/step[1];

       del_accu  (pntra, npix, step, fwhm_del, orgcol, xmax, ymax, 
                                  ntrace, widtrace);

       nord++;
       TCRWRR(tid, nord, ncol, icol, outr);

       sprintf(text, "Detect. order %d, slope %f, interc. %f, fwhm %f",nord,
           outr[0],outr[1],outr[3]);
       SCTPUT(text);

       /* Find  next maximum and test the end condition */

       if (nord <= 3) 
          find_max (pntra, npix, &xmax, &ymax, &peak);
       else {
          LSfilter(intercept,slope,nord,&sl,&inter,&rms);
          find_window_max(pntra, npix, &xmax, &ymax, &peak, 
                          sl, inter, sl_window);
	}

       keep_on_working=((peak > lowmax && nord < minmax[1]) || 
                 (nord < minmax[0]));

  } /* Matches while (keep_on_working) */

    TCTCLO(tid);
    SCSEPI();

  }

find_max (pntra, npix, xmax, ymax, peak)

    float    *pntra;
    int npix[2];
    float    *xmax, *ymax, *peak;

{

    int      nrow, ncol, posmax, pos, pmax;

    nrow = npix[1]-1;
    ncol = npix[0]-1;

/* Init */

    *peak = pntra[0];
    posmax = ipos(ncol,nrow,npix[0]);
    pmax = 0;

/* Find the maximum */

    for (pos=1; pos<=posmax; pos++) {  

               if (pntra[pos] > *peak) {
                   *peak = pntra[pos];
                   pmax = pos;
		 }
	    }
    *ymax = (int) (pmax/npix[0]);
    *xmax = (int) (pmax - *ymax * npix[0]);
}

find_window_max (pntra, npix, xmax, ymax, peak, a, b, sl_w)

    float    *pntra, a, b;
    int npix[2], sl_w;
    float    *xmax, *ymax, *peak;

{

    int      row, col, pos, pmax, flag=0;
    int      col_start, col_end;

/* Find the maximum */

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

         col_start = (int) (a + b*row - sl_w);
         col_end   = col_start + 2*sl_w;

         if (col_start < 0) col_start=0;
         if (col_end >= npix[0]) col_end = npix[0]-1;

         for (col=col_start; col<=col_end; col++) {
               pos = ipos(col,row,npix[0]);
               if (flag == 0) {flag=1; *peak=pntra[pos];}
               else {
                  if (pntra[pos] > *peak)
                     {*peak = pntra[pos]; pmax = pos;}
		}
	     }
       }
    *ymax = (int) (pmax/npix[0]);
    *xmax = (int) (pmax - *ymax * npix[0]);
}

/* LSfilter performs linear regression  vy = a + b vx */
LSfilter(vx, vy, n, a, b, rms)

float vx[], vy[], *a, *b, *rms;
int   n;

{
  double x,y,cnt,sx,sy,sx2,sxy,sy2;
  double det;
  int    i;

  x=y=cnt=sx=sy=sx2=sxy=sy2=0.;

  for (i=0; i<n; i++) {
       x = vx[i], y = vy[i];
       cnt += 1.;
       sx+=x, sy+=y, sx2+=x*x, sy2+=y*y, sxy+=x*y;
     }

  if (cnt >= 2) {
       det = cnt*sx2 - sx*sx;
       *a   = (sy*sx2 - sx*sxy)/det;
       *b   = (cnt*sxy - sx*sy)/det;
       *rms = (sy2 - (*a)*(*a)*cnt - 2.*(*b)*(*a)*sx - 
              (*b)*(*b)*sx2)/cnt;
     }
   else
       *rms = -999., *a=0., *b=0.;

}

find_cntr (pntra, npix, orgcol, xmax, ymax, peak, step, fwhm, 
                                                thres, xcntr, ycntr)

    float    *pntra;
    int      npix[2];
    double   xmax, ymax, peak, thres, fwhm;
    double   step[2];
    float    *xcntr, *ycntr;
    int      orgcol;

{

    int         nrow, ncol, row, col, pos, offset;
    float       slope, orig, row_cntr;
    double      scol=0., srow=0., cnt=0.;
    int         hw;

    nrow = npix[1];
    ncol = npix[0];

    slope = step[0]*orgcol/(-2.)/step[1];
    orig  = ymax - slope*xmax;
    hw    = (int) (fwhm + 0.5);

    for (col=0; col<ncol; col++) {

        row_cntr = slope*col + orig + 0.5;

        for (offset = -hw; offset<=hw; offset++) {

            row = (int) (row_cntr + offset);

            if (row>=0 && row<nrow) {

                 pos = ipos(col, row, ncol);

                 if (pntra[pos] > thres*peak) {
                      scol = scol + pntra[pos]*col;
                      srow = srow + pntra[pos]*row;
                      cnt  = cnt  + pntra[pos];
		    }

	       }
	  }
      }

    *xcntr = (float) (scol/cnt);
    *ycntr = (float) (srow/cnt);
}
    

find_width (pntra, npix, xmax, ymax, peak, fwhm)

    float    *pntra;
    int      npix[2];
    double   xmax, ymax, peak;
    float    *fwhm;

{

    int      nrow, ncol, row, col, i, imax=2;
    int      upper_limit=0, lower_limit=0, crow;
    float    value, vmax, vmax_ref, val_low, val_upp;

    nrow  =  npix[1];
    ncol  =  npix[0];
    col   =  (int) (xmax + 0.5);

     row = (int) ymax;
     for (vmax=0., i=(-imax); i<=imax; i++) {
             crow = row + i;
             vmax += pntra[ipos(col, crow, ncol)];
     }
     vmax_ref = vmax;

    for  (row=(int)ymax; row<nrow; row++) {

          value=0., val_low=0., val_upp=0.;
          for (i=(-imax);i<=imax;i++) {
             crow = row + i;
             value += pntra[ipos(col, crow, ncol)];
             crow = row - imax + i;
             val_low += pntra[ipos(col, crow, ncol)];
             crow = row + imax + i;
             val_upp += pntra[ipos(col, crow, ncol)];
          }

          if (value <= val_low && value <= val_upp) 
                 {upper_limit = row; break;}
	}

    vmax = vmax_ref;

    for  (row=(int)ymax; row>0; row--) {

          value=0., val_low=0., val_upp=0.;
          for (i=(-imax);i<=imax;i++) {
             crow = row + i;
             value += pntra[ipos(col, crow, ncol)];
             crow = row - imax + i;
             val_low += pntra[ipos(col, crow, ncol)];
             crow = row + imax + i;
             val_upp += pntra[ipos(col, crow, ncol)];
          }

          if (value <= val_low && value <= val_upp) 
                 {lower_limit = row; break;}
	}

    *fwhm = (float) (upper_limit - lower_limit)/2. - 1.;
    if (*fwhm < 0.) *fwhm = 0.;

  }


del_accu (pntra, npix, step, fwhm, orgcol, xmax, ymax, ntrace, width)

    float    *pntra;
    int      npix[2];
    double   xmax, ymax, fwhm, step[2];
    int      orgcol, ntrace, width;

{

    int         nrow, ncol, row, col;
    float       slope;
    float       orig, row_cntr;
    int         hw, trace, colref;
    int         pos, posmin, posmax;

    nrow  =   npix[1];
    ncol  =   npix[0];

    hw    = (int) (fwhm);

    for (trace=1; trace<=ntrace; trace++) {

        colref = orgcol/2. -0.5 + (trace - ((ntrace+1.)/2.))*width;
        slope = step[0]*(-1.)*colref/step[1];
        orig  = ymax - slope*xmax;

        for (col=0; col<ncol; col++) {

           row_cntr = slope*col + orig + 0.5;

           row      = (int) (row_cntr - hw);
           if (row < 0)  row = 0;             /* Max(row,0)  */
           posmin   = ipos(col,row,ncol);

           row      = (int) (row_cntr + hw);
           if (row >= nrow) row = nrow - 1;  /* Min(row,nbrow-1) */
           posmax   = ipos(col,row,ncol);

           for (pos=posmin; pos<=posmax; pos+=ncol) 
                   pntra[pos] = 0.;
	 }
      }
}



double correlate(pntr, npix, x, y, shift, shift_max)

float *pntr;
int   npix[2], x, y, shift, shift_max;

{
  int row, pos, nrow, ncol, crow[2];
  double xcorr;

  nrow = npix[1];
  ncol = npix[0];
  crow[0] = shift_max;
  crow[1] = nrow;

  pos  = ipos(x, crow[0], ncol);
  xcorr = 0.;

  for (row=crow[0]; row<crow[1]; row++) {
        xcorr += pntr[pos] * pntr[pos-shift*ncol];
        pos   += ncol;
      }
  return(xcorr);
}

cor_find_width(pntra, npix, xmax, ymax, peak, fwhm)

    float    *pntra;
    int      npix[2];
    double   xmax, ymax, peak;
    float    *fwhm;

{
    double   value, vprev;
    int      shift, shift_max, nrow, col, row;

    nrow = npix[1];

    col = (int) (xmax+0.5);
    row = (int) (ymax+0.5);

    shift_max = (nrow > 200) ? 100 : (nrow/2);
    shift     = 1;

    vprev = correlate(pntra, npix, col, row, shift, shift_max);
    shift += 1;

    while (vprev > (value=correlate(pntra, npix, col, row, 
                    shift, shift_max)) && (shift<shift_max))
           { vprev = value; shift += 1;}

    *fwhm = shift-1;
  }