/* @(#)wavecal.c	17.1.1.1 (ESO-IPG) 01/25/02 17:52:00 */
/* 

   Anton Malina, Otmar Stahl

   wavecal.c

   global fit to wavelengths

*/

/* system includes */

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

/* general Midas includes */

#include <midas_def.h>

/* FEROS specific includes */

#include <misc.h>
#include <proto_nrutil.h>
#include <2Dstuff.h>
#include <u_const.h>

#define NRANSI
#define JMAX 20
#define JMAXP (JMAX+1)
#define K 10
#define BOUND 10.0

#define EPS 0.00003
void fit_wavelength
#ifdef __STDC__
(
 double inimage[], double ximage[], double yimage[], int npixi,
 double outimage[], double ximageo[], double yimageo[], int npixo,
 int fit_deg, double a[], int printout
 )
#else
     (
      inimage, ximage, yimage, npixi, outimage, ximageo, yimageo, npixo, 
      fit_deg, a, printout
      )
     double inimage[], ximage[], yimage[], outimage[], ximageo[], yimageo[];
     int npixi, npixo, fit_deg, printout;
     double a[];
#endif
{
  double x1out, x2out;
  int i, iter = 0;
  double *sig, **covar, **alpha, alamda, chisq = 0.0, ochisq = 0.0;
  int *ia;
  char info[80];
  
  sig = dvector(1, npixi);
  ia = ivector(1, fit_deg);
  covar = dmatrix(1, fit_deg, 1, fit_deg);
  alpha = dmatrix(1, fit_deg, 1, fit_deg);

  for (i = 1; i <= npixi; i++)
    {
      sig[i] = (double) 1.0;
    }
  
  for (i = 1; i <= fit_deg; i++)
    {
      ia[i] = 1;
    }

  /* 

     Experiment has shown that the following coefficients can be set to zero,
     since their inclusion leads to marginally better residuals only.

     The number of parameters is reduced from 25 to 15, 
     which hopefully improves stability.
     This needs some further tests, however.
     
     Otmar Stahl, 23-FEB-1999

   */
    
  a[13] = 0.0; /* x2 * m2 */
  a[14] = 0.0; /* x2 * m3 */
  a[15] = 0.0; /* x2 * m4 */

  a[18] = 0.0; /* x3 * m2 */
  a[19] = 0.0; /* x3 * m3 */
  a[20] = 0.0; /* x3 * m4 */

  a[22] = 0.0; /* x4 * m1 */
  a[23] = 0.0; /* x4 * m2 */
  a[24] = 0.0; /* x4 * m3 */
  a[25] = 0.0; /* x4 * m4 */

  ia[13] = 0;
  ia[14] = 0;
  ia[15] = 0;

  ia[18] = 0;
  ia[19] = 0;
  ia[20] = 0;

  ia[22] = 0;
  ia[23] = 0;
  ia[24] = 0;
  ia[25] = 0;


  alamda = -1.0;

  if (printout > 1)
    {
      sprintf(info, "fit_wavelength: initial values");
      SCTPUT(info);
      for(i = 1; i <= fit_deg; i++)
	{
	  sprintf(info, "a[%i]=%e  ", i, a[i]);
	  SCTPUT(info);
	}
      sprintf(info, "\n");
      SCTPUT(info);
    }
  
  mrqmin2D(ximage, yimage, inimage, sig, npixi, a, ia, fit_deg, covar, alpha,
	   &chisq, deriv_OS, &alamda); 
  
  ++iter;
  
  if (printout > 1) {
    for(i = 1; i <= fit_deg; i++)
      {
	sprintf(info, "a[%i]=%e  ", i, a[i]);
	SCTPUT(info);
      }
    sprintf(info, "\nfit_wavelength: %2d iterations %7d lines ==> chi = %9.7f\n", 
	    iter, npixi, sqrt(chisq));  
    SCTPUT(info);
  }

  do
    {
      ochisq = chisq;

      mrqmin2D(ximage, yimage, inimage, sig, npixi, a, ia, fit_deg, covar, 
	       alpha, &chisq, deriv_OS, &alamda); 
      ++iter;
      
      if (printout > 1)
	{
	  for(i = 1; i <= fit_deg; i++)
	    {
	      sprintf(info, "a[%i]=%e  ", i, a[i]);
	      SCTPUT(info);
	    }
	  sprintf(info, "\n");
	  SCTPUT(info);
	  sprintf(info, "fit_wavelength: %2d iterations %7d lines ==> chi = %9.7f\n",
		  iter, npixi, sqrt(chisq));
	  SCTPUT(info);
	}
      
    } while (fabs(ochisq - chisq) / chisq > EPS); 

  alamda = 0.0;  /* To de-allocate memory */
  mrqmin2D(ximage, yimage, inimage, sig, npixi, a, ia, fit_deg, covar, 
	   alpha, &chisq, deriv_OS, &alamda); 

  if (printout > 0)
    {
      sprintf(info, "\nfit_wavelength: final values\n");
      SCTPUT(info);
      for(i = 1; i <= fit_deg; i++)
	{
	  sprintf(info, "a[%i]=%e  ", i, a[i]);
	  SCTPUT(info);
	}
  
      sprintf(info, "\nfit_wavelength: %2d iterations %7d lines ==> chi = %9.7f\n",
	      iter, npixi, sqrt(chisq));
      SCTPUT(info);
    }

  for (i = 1; i <= npixo; i++)
    {
      x1out = (double) ximageo[i];
      x2out = (double) yimageo[i];
      outimage[i] = (double) eval_OS(x1out, x2out, a, fit_deg);
    }

  free_dvector(sig, 1, npixi);
  free_ivector(ia, 1, fit_deg);
  free_dmatrix(covar, 1, fit_deg, 1, fit_deg);
  free_dmatrix(alpha, 1, fit_deg, 1, fit_deg);
  /* fit_wavelength */
}
#undef EPS

void deriv_OS
#ifdef __STDC__
(
 double x, double m, double a[], double *y, double dyda[], int n
 )
#else
     (
      x, m, a, y, dyda, n
      )
     double x, m, a[], *y, dyda[];
     int n;
#endif
{
  double x1, x2, x3, x4, m1, m2, m3, m4;
  int i;
  
  m1 = m;
  m2 = m1 * m;
  m3 = m2 * m;
  m4 = m3 * m;
  /*  m5 = m4 * m; */

  x1 = x;
  x2 = x1 * x;
  x3 = x2 * x;
  x4 = x3 * x;

  /*  
  dyda[1] = 1.0/m;
  dyda[2] = m1/m;
  dyda[3] = m2/m;
  dyda[4] = x1/m;
  dyda[5] = x2/m;
  */

  dyda[1] = 1.0;
  dyda[2] = m1;
  dyda[3] = m2;
  dyda[4] = m3;
  dyda[5] = m4;

  dyda[6]  = x1;
  dyda[7]  = x1 * m1;
  dyda[8]  = x1 * m2;
  dyda[9]  = x1 * m3;
  dyda[10] = x1 * m4;

  dyda[11] = x2;
  dyda[12] = x2 * m1;
  dyda[13] = x2 * m2;
  dyda[14] = x2 * m3;
  dyda[15] = x2 * m4;

  dyda[16] = x3;
  dyda[17] = x3 * m1;
  dyda[18] = x3 * m2;
  dyda[19] = x3 * m3;
  dyda[20] = x3 * m4;
  
  dyda[21] = x4;
  dyda[22] = x4 * m1;
  dyda[23] = x4 * m2;
  dyda[24] = x4 * m3;
  dyda[25] = x4 * m4;

  for (i=1; i<=25; i++)
    {
      dyda[i] = dyda[i]/m;
    }

  *y = eval_OS(x, m, a, n);
  

  return;
} /* deriv_OS */


double eval_OS
#ifdef __STDC__
(
 double x, double m, double a[], int n
 )
#else
     (
      x, m, a, n
      )
     double x, m, a[];
     int n;
#endif
{
  double ret_val;
  double x1, x2, x3, x4, m1, m2, m3, m4;

  m1 = m;
  m2 = m1 * m;
  m3 = m2 * m;
  m4 = m3 * m;

  x1 = x;
  x2 = x1 * x;
  x3 = x2 * x;
  x4 = x3 * x;

  /*  
  ret_val = (a[1] + a[2] * m1 + a[3] * m2 + a[4] * x1 + a[5] * x2)/m;
  return ret_val;
  */

  ret_val =  a[1]  + a[2] *m1 + a[3] *m2 + a[4]* m3 + a[5] *m4       +
            (a[6]  + a[7] *m1 + a[8] *m2 + a[9]* m3 + a[10]*m4) * x1 +
            (a[11] + a[12]*m1 + a[13]*m2 + a[14]*m3 + a[15]*m4) * x2 +
            (a[16] + a[17]*m1 + a[18]*m2 + a[19]*m3 + a[20]*m4) * x3 +
            (a[21] + a[22]*m1 + a[23]*m2 + a[24]*m3 + a[25]*m4) * x4 
    ;
  
  ret_val = ret_val / m;
  
  return ret_val;
  
} /* eval_OS */


int global_to_poly 
#ifdef __STDC__
(
 double dcoef[], double polcoeff[], int order1, int order2, int poldeg
 )
#else
     (
      dcoef, polcoeff, order1, order2, poldeg
      )
     double dcoef[], polcoeff[];
     int order1, order2, poldeg;
#endif
{     
  double m1, m2, m3, m4;
  int i, k;

  k = 1;
  for (i=order2; i>=order1; i--)
    {
      m1 = i;
      m2 = m1 * i;
      m3 = m2 * i;
      m4 = m3 * i;

      polcoeff[(k-1)*poldeg+1] = 
	(dcoef[1] + dcoef[2] * m1 + dcoef[3] * m2 + 
	 dcoef[4] * m3 + dcoef[5] * m4) / m1;

      polcoeff[(k-1)*poldeg+2] = 
	(dcoef[6] + dcoef[7] * m1 + dcoef[8] * m2 + 
	 dcoef[9] * m3 + dcoef[10] * m4) / m1;

      polcoeff[(k-1)*poldeg+3] = 
	(dcoef[11] + dcoef[12] * m1 + dcoef[13] * m2 + 
	 dcoef[14] * m3 + dcoef[15] * m4) / m1;
      
      polcoeff[(k-1)*poldeg+4] = 
	(dcoef[16] + dcoef[17] * m1 + dcoef[18] * m2 + 
	 dcoef[19] *m3 + dcoef[20] * m4) / m1;

      polcoeff[(k-1)*poldeg+5] = 
	(dcoef[21] + dcoef[22] * m1 + dcoef[23] * m2  + 
	 dcoef[24] * m3 + dcoef[25] * m4) / m1;
      k++;
    }
  return 0;
  }

double deg_to_rad
#ifdef __STDC__
(
 double argument
 )
#else
     (
      argument
      )
     double argument;
#endif
{
  return argument / 180. * PI;
}