/* @(#)wavecal.c	19.1 (ESO-IPG) 02/25/03 14:21:27 */
/* 

   Anton Malina, Otmar Stahl

   wavecal.c

   global fit to wavelengths

*/

/* before other includes as some define in the following breaks macos 10.10 stdlib.h */
#include <stdlib.h>

/* general Midas includes */

#include <midas_def.h>

/* FEROS specific includes */

#include <misc.h>
#include <nrutil.h>
#include <mpfit.h>
#include <u_const.h>

/* system includes */

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


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


typedef struct {
    double * x;
    double * y;
    double * z;
} priv_data;

int OS_with_derivs(int ndata, int npar, double *p, double *deviates,
                        double **derivs, priv_data *d)
{
    int i;
    double * x = d->x;
    double * y = d->y;
    double * z = d->z;
    double * dyda = malloc(npar * sizeof(*dyda));

    /* Compute function deviates */
    for (i=1; i<=ndata; i++) {
        double v;
        deriv_OS(x[i], y[i], p, &v, dyda, npar);
        deviates[i - 1] = z[i] - v;
        if (derivs) {
            int j;
            for (j=1; j<npar; j++) {
                if (derivs[j]) {
                    derivs[j][i - 1] = -dyda[j];
                }
            }
        }
    }
    free(dyda);
    return 0;
}

void fit_wavelength
(
 double inimage[], double ximage[], double yimage[], int npixi,
 double outimage[], double ximageo[], double yimageo[], int npixo,
 int fit_deg, double ain[], int printout
 )
{
    int i;
    char info[80];
    priv_data pdata = {ximage, yimage, inimage};
    double * a = malloc((fit_deg + 1) * sizeof(*a));
    mp_par * pars = calloc(fit_deg + 1, sizeof(*pars));
    mp_result result;

    memset(&result, 0, sizeof(result));
    memcpy(&a[1], &ain[1], (fit_deg) * sizeof(*a));
    /* extra parameter for fortran indexing
       would be invalid write in ain from dvector */
    a[0] = 0.;
    pars[0].fixed = 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
    */

    pars[13].fixed = 1;
    pars[14].fixed = 1;
    pars[15].fixed = 1;

    pars[18].fixed = 1;
    pars[19].fixed = 1;
    pars[20].fixed = 1;

    pars[22].fixed = 1;
    pars[23].fixed = 1;
    pars[24].fixed = 1;
    pars[25].fixed = 1;
    for (i = 0; i < fit_deg + 1; i++) {
        pars[i].side = 3;
    }

    /* deriv_OS is a linear function, unknown why a non-linear fit is used */
    mpfit((mp_func)&OS_with_derivs, npixi, fit_deg + 1,
                  a, pars, NULL, &pdata, &result);

    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",
                result.niter, npixi, sqrt(result.bestnorm));
        SCTPUT(info);
    }

    for (i = 1; i <= npixo; i++) {
        double x1out = (double) ximageo[i];
        double x2out = (double) yimageo[i];
        outimage[i] = (double) eval_OS(x1out, x2out, a, fit_deg);
    }
    memcpy(&ain[1], &a[1], (fit_deg) * sizeof(*a));
    free(a);
    free(pars);
}

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;
}

