/* @(#)center_one_order.c	17.1.1.1 (ESO-IPG) 01/25/02 17:51:58 */
/* 

   Otmar Stahl

   center_one_order.c  

   center one order image 

*/

/* system includes */

#include <math.h>

/* general Midas includes */

#include <midas_def.h>

/* FEROS specific includes */

#include <echelle.h>
#include <proto_nrutil.h>
#include <proto_mutil.h>
#include <proto_fitnol.h>
#include <misc.h>

int center_one_order
#ifdef __STDC__
(
 float imageo[], float imaget[], float corr_arr[], 
 int center, int npixt[], 
 int width2, int order, int lrspan,
 int lnfit_method, float *shift, float *maxi, int *max_index
 )
#else
     (
      imageo, imaget, corr_arr, center, npixt, 
      width2, order, lrspan, shift, lnfit_method, maxi, max_index
      )
     
     float imageo[], imaget[], corr_arr[];
     int center, npixt[], width2, order, *max_index, lrspan, lnfit_method;
     float *shift, *maxi;
#endif

{
  float mini, xcorr, aleft, aright, factor, a, b;
  int jj, k, first, last, index;

  double *xpositions;
  double gaussparams[4]; /* A, x0, sigma */
  double *fold;

  xpositions = dvector(0,2*lrspan+2);

  fold = dvector(1, 2*lrspan+1); /* must start at 1; fit_gauss expects it */

  /*   normalize  first */

  mini = 3.0e34;	  
  for(jj = -width2; jj <= width2; jj++) 
    {
      if (mini > imageo[center + jj])
	{
	  mini = imageo[center + jj];
	}
    }
  xcorr = 0.0;
  for(jj = -width2; jj <= width2; jj++)
    {
      xcorr += SQUARE(imageo[center + jj] - mini);
    }
  xcorr = (float) sqrt((double) xcorr);
  if (xcorr < 0.001) xcorr = 0.001;            /* avoid division by zero */

  for(jj = -width2; jj <= width2; jj++)
    {	      
      corr_arr[jj + width2] = (imageo[center + jj] - mini) / xcorr;
    }

  /* now compute correlation */

  for(k = -lrspan; k <= lrspan ; k++)
    {      
      first = -width2;
      if(k < 0) first = -width2 - k;
      last = width2;
      if(k > 0) last = width2 - k;
      fold[k + lrspan+1] = 0.0;
      for (jj = first; jj <= last; jj++) 
	{


	  fold[k + lrspan+1] = 
	    fold[k + lrspan+1] +
	    imaget[npixt[0] * order + jj + width2] * 
	    corr_arr[jj + k + width2];

	  /* 
	     fold[] contains the cross-correlation of the COP 
	     with a template 
	  */

	}
    }

  /* search maximum */

  *maxi = -3.0e34;  /* initial value */
  index = lrspan+1; /* initial value */

  for (k = 1; k <= 2*lrspan+1; k++)
    {
      if (fold[k] > *maxi)
	{ 
	  index = k;
	  *maxi = fold[k];             
	  *max_index = k - (lrspan+1); 

	  /* *maxi is the biggest array element 
	     and *max_index is the index
	     of first element of the array
	     with *maxi as the middle element 
	  */
	}
    }

  switch (lnfit_method)
    {
    case GAUSSIAN:

      /* 
	 gaussian centering 
	 the fit routine requires initial estimates 
	 for the gauss-parameters 
      */  
      
      for(k = 1; k <= 2*lrspan+1; k++)
	xpositions[k] = k;
      
      gaussparams[1] = 1.0;
      gaussparams[2] = (double) index;
      gaussparams[3] = 3.6;
      fit_gauss(xpositions, fold, 2*lrspan+1, gaussparams, 3);
      *shift = gaussparams[2] - (double) index;
      
      break;
    case GRAVITY:
      
      /* center of gravity */
      
      aleft = fold[index - 1];
      aright = fold[index + 1];
      factor = 1;
      if (aleft >= aright)
	{
	  aleft = fold[index + 1];
	  aright = fold[index - 1];
	  factor = -1;
	}
      
      a = fold[index] - aleft;
      b = aright - aleft;
      *shift = (a + b == 0.0) ? 0.0 : b / (a + b);
      
      break;
    }

  free_dvector(fold, 1, 2*lrspan+1);
  return 0;
}