/* @(#)libhough.c	17.1.1.1 (ES0-DMD) 01/25/02 17:55:51 */
/*===========================================================================
  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
===========================================================================*/


#include <math.h>
#include <midas_def.h>
#include <osparms.h>
#include <time.h>
#include <ok.h>

#define    ipos2D(col,row,npix)          row*npix[0]+col
#define    ipos3D(col1,row,col2,npix)    (col2*npix[1]+row)*npix[0]+col1

#ifndef    M_PI
#define    M_PI    3.14159265358979323846
#endif

float          *pntr;
int            dimension, imno;
int            npix_hg[3], check_range();
double         start_hg[3], step_hg[3], end_hg[3], fct();

void create_hough(name, npix, start, step, dim)

char *name;
int      npix[], dim;
double   start[], step[];

{
    char           ident[81], cunit[65];
    int            i, kunit;
    int            row, nrow;

    dimension = dim;

     for(i=0; i<((dim+1)*16); i++) cunit[i]=' ';
     cunit[((dim+1)*16)] = '\0';
 
     if (dim == 3)
        strcpy(ident,"3D Hough Transform Image");
   
     if (dim == 2)
        strcpy(ident,"2D Hough Transform Image");
   
     if (dim == 1)
        strcpy(ident,"1D Hough Transform Image");
   
     if (name[0] != '@') 

     SCIPUT (name, D_R4_FORMAT, F_O_MODE, F_IMA_TYPE,
                dimension, npix, start, step, ident, cunit, (char **)&pntr,
                &imno);
     else

     SCIPUT ("midd.bdf", D_R4_FORMAT, F_X_MODE, F_IMA_TYPE,
                dimension, npix, start, step, ident, cunit, (char **)&pntr,
                &imno);

     for (i=0; i<dimension; i++) {
        npix_hg[i]  = npix[i];
        start_hg[i] = start[i];
        step_hg[i]  = step[i];
        end_hg[i]   = start[i] + step[i]*(npix[i]-1);
      }

   nrow = npix_hg[0];
   if (dimension >= 2) nrow *= npix_hg[1];
   if (dimension >= 3) nrow *= npix_hg[2];

   for (row=0; row<nrow; row++)  pntr[row] = 0;

}

void close_hough()

{

SCFCLO(imno);

}


void increment_hough(x,y,increment,mode,disp,range)

double  x,y;
float   increment,disp,range;
char    mode[];

{

double coef0, coef1, coef2; 
int    col, col1, col2, row, index;
double inc, incupp, inclow, drow, delta;
int    rowupp,rowlow;

  mode[0] = toupper(mode[0]);

  if ( (mode[0] == 'L') || (mode[0] == 'N') ) {
    for (col=0; col<npix_hg[0]; col++) {
      coef1   = start_hg[0] + col*step_hg[0];
      if (mode[0] == 'L')  {
             coef0   = y - coef1*x;
             delta   = 2.*coef1*range;
	   }

      if (mode[0] == 'N')  {
             coef0   = y - x*disp*(1.+x*coef1);
             delta   = 2.*disp*range*(1.+2*coef1*x);
	   }

     /* printf("x,y, coef1, coef0: %f %f %f %f\n",x,y,coef1,coef0); */
    if (check_range(coef0,start_hg[1],end_hg[1])) {

     if (range < 0.) delta = (double) range;
     if (delta < 0.) delta *= -1.;
     delta /= step_hg[1];

     drow    =   (coef0 - start_hg[1])/step_hg[1];

     rowupp  =   (int) (drow + 0.5 + delta);
     rowlow  =   (int) (drow + 0.5 - delta);

    if (rowupp >= npix_hg[1]) rowupp =  npix_hg[1] - 1;
    if (rowlow < 0 )          rowlow =  0;

    if (rowlow <= rowupp) {
    for (row=rowlow; row<=rowupp; row++) {
         if (delta > 0)  inc = fct ((row-drow), delta);
         else            inc = 1.;
         pntr[ipos2D(col,row,npix_hg)] += inc*increment;
       }
  }}

  }}

  if (mode[0] == '1') {
      coef0 = y - x*disp;
      if (check_range(coef0,start_hg[0],end_hg[0])) {
      drow  = (coef0 - start_hg[0])/step_hg[0];
      rowupp  =   (int) (drow + 0.5);
      incupp  =   drow + 0.5 - rowupp;
      inclow  =   1. - incupp;
      rowlow  =   rowupp - 1;

    if (rowupp >= 0 && rowupp < npix_hg[0]) /* Add = */
      pntr[rowupp] += incupp*increment;

    if (rowlow >= 0 && rowlow < npix_hg[0]) /* Add = */
      pntr[rowlow] += inclow*increment;

    }}


  if ( (mode[0] == '3') ) {

    for (col1=0; col1<npix_hg[0]; col1++) {
    for (col2=0; col2<npix_hg[2]; col2++) {
      coef1   = start_hg[0] + col1*step_hg[0];  /* Dispersion */
      coef2   = start_hg[2] + col2*step_hg[2];  /* 2nd order coeff. */

      coef0   = y - x*coef1*(1.+x*coef2);
      /* printf("x,y, coef1, coef0: %f %f %f %f\n",x,y,coef1,coef0); */
      if (check_range(coef0,start_hg[1],end_hg[1])) {
      delta   = 2.*coef1*range*(1.+2*coef2*x);

     if (range < 0.) delta = (double) range;
     if (delta < 0.) delta *= -1.;
     delta /= step_hg[1];

      drow    =   (coef0 - start_hg[1])/step_hg[1];
      rowupp  =   (int) (drow + 0.5 + delta);
      rowlow  =   (int) (drow + 0.5 - delta);

    if (rowupp >= npix_hg[1]) rowupp =  npix_hg[1] - 1;
    if (rowlow < 0 )          rowlow =  0;

    if (rowlow <= rowupp) {

       index = ipos3D(col1,rowlow,col2,npix_hg);

       if (delta > 0) {
          for (row=rowlow; row<=rowupp; row++) {
             inc = fct((row-drow), delta)*increment;
             pntr[index] += inc;
             index += npix_hg[0];
          }
	}

       else {
          for (row=rowlow; row<=rowupp; row++) {
             pntr[index] += increment;
             index += npix_hg[0];
          }
       }

     }

  }}}
  }


}

float findmax(x,y,z)

int      *x,*y,*z;

{

   int      nrow, row, posmax;
   float max;

   nrow = npix_hg[0];
   if (dimension >= 2) nrow *= npix_hg[1];
   if (dimension >= 3) nrow *= npix_hg[2];

   max = pntr[0];

   for (row=0; row<nrow; row++)  {
       if (pntr[row] >= max) {
           max = pntr[row];
           posmax = row;
	 }}

   *x = *y = *z = 0;

   switch(dimension) {

   case(1) :  {
        *x = (int) (posmax + 0.5);
        break;
      }

   case(2) : {
        *y = (int) (posmax/npix_hg[0] + 0.5);
        *x = (int) (posmax - *y*npix_hg[0] + 0.5);
        break;
      }

   case(3): {
        *z = (int) (posmax/npix_hg[0]/npix_hg[1] + 0.5);
        *y = (int) (posmax/npix_hg[0] -*z*npix_hg[1] + 0.5);
        *x = (int) (posmax - (*z*npix_hg[1]+*y)*npix_hg[0] + 0.5);
        break;
      }

 }

*x += 1;
*y += 1;
*z += 1;

return(max);

}


double fct(r,delta)

double r,delta;

{
double result, absr;

absr = (r < 0.) ? (-1.)*r : r;

if (absr < delta) result = cos (r*M_PI/2./delta);
else              result = 0.;

/* result = cos (r*PI/2./delta)   */
/* result = delta*delta  - r*r    */
/* result = 1. - r*r/delta/delta  */

return(result);

}

check_range(value,min,max)

double value, min, max;

{

if (value < min || value > max) return(FALSE);
else                            return(TRUE);

}