/* +++++++++++++++++++++++++++++++++++++++++++++++++++++++ */
/* .COPYRIGHT   (C) 1993 European Southern Observatory     */
/* .IDENT       mosdisp.c                                  */
/* .AUTHORS     Pascal Ballester (ESO/Garching)            */
/*              Cristian Levin   (ESO/La Silla)            */
/* .KEYWORDS    Spectroscopy, Long-Slit, MOS               */
/* .PURPOSE                                                */
/* .VERSION     1.0  Package Modification 21-SEP-1993      */
/* ------------------------------------------------------- */

/* Purpose:
   This module is related to the dispersion relation and its representation.
   The upper modules (moscalib.c, mosrebin.c) do not know which representation
   is used (Legendre polynomials, or any other), nor how it is stored (which
   descriptor, meaning of coefficients). This design enables to replace 
   this module mosdisp.c by any other based on a different representation.
*/

/* Note:
`
   Because the module is based on Numerical Recipes routines, which arrays
   start at index 1, the general rule is that arrays start at index 1. However
   the module can accomodate any starting index, which is specified at 
   initialisation time (function initdisp).
*/


#include <math.h>
#include <midas_def.h>
#include <tbldef.h>
#include <proto_fitnol.h>
#include <proto_mos.h>

#ifndef PI 
#define PI 3.141592653589793
#endif

#define MAXNCOE 100
#ifdef __STDC__
  void fleg(double, double [], int);
  void lfit2(double [], double [], double [], int, double [],
           int, int [], int, double **, double *,
           void (*funcs)(double, double *, int));
  void     fpoly(double, double [], int); 
  int      *ivector(int, int);
  double   **dmatrix(int, int, int, int), *dvector( int, int);
  void     free_dmatrix(double **, int, int, int, int);
  void     free_dvector(double *, int, int), free_ivector(int *, int, int);
  void     gaussj(double **, int, double **, int ); 	
  void     covsrt(double **, int, int [], int );
  void     nrerror(char []);
#else
  void   fleg();
  void   lfit2();
  void   fpoly();
  int    *ivector();
  double **dmatrix();
  void   free_dmatrix();
  void   free_dvector();
  void   gaussj();
  void   covsrt();
  void   nerror();
#endif
/* void     (*function) (), function = fpoly;   */
/* Another way to define a variable pointing to a function */

double   pixbin;              /* Pixel bin in wavelength units */
double   coef[100];           /* Array of coefficients  */
int	 fdeg, ncoef, refdeg, maxcoef; /* Number of coefficients, Degree */
int      degy,degxy; /* order of polynoms in y-dir and of the mixed term */
int	 tide, colslit, colline, coly, colrms, colcoef[100];
int	 start_index, nbline;


/* -------------------------------------------------------------------------- */
#ifdef _STDC__
  void setrefdeg_2D(int deg, int deg1, int deg2)
#else
  void setrefdeg_2D(deg, deg1, deg2) 
  int deg;
#endif
{

refdeg = deg;
degy = deg1;
degxy = deg2;
maxcoef = refdeg + degy + degxy + 1;
}

/* -------------------------------------------------------------------------- */
#ifdef _STDC__
  void setdisp_2D(int deg, double coefs[])
#else
  void setdisp_2D(deg, coefs)
  int deg;
  double  coefs[];
#endif
{

int i;

fdeg = deg;
refdeg = deg;
ncoef = deg + degy + degxy + 1;
maxcoef = deg + degy + degxy + 1;

for (i=1; i<=ncoef; i++) 
  coef[i] = coefs[i-1];
}


/* -------------------------------------------------------------------------*/


#ifdef _STDC__
  double mos_fit_disp_2D(int *ndata, int *deg, double x[], double y[],
		       double l[], double *chi)
#else
  double mos_fit_disp_2D(ndata, deg, x, y, l, chi)
  int *ndata, *deg;
  double x[],y[],l[],*chi;
#endif
{

  double   *sig, **covar, *chisq, *a, *xsh, *ysh, *lsh;
  int *lista;
  int i;
  int maxdeg,actvals;
  char poltyp[8];

  /* printf("NData : %d   ---   Deg value : %d\n",*ndata,*deg); */

  refdeg = *deg;
  maxdeg = *ndata ;
  if (refdeg > maxdeg) fdeg = maxdeg;
  else                 fdeg = refdeg;
  ncoef   = fdeg + degy + degxy + 1;
  maxcoef = fdeg + degy + degxy + 1;

  if (*ndata < ncoef) {
    printf("Not enough lines (minimum is 2). \nNo dispersion relation computed\n");
    return(-2.);
  }
  
  if (fdeg < 1)  {
    printf("Degree : %d. No dispersion relation fitted\n",*deg);
    return(-1.);
  }
  

  covar=dmatrix(1,*ndata,1,*ndata);
  chisq=(double*) dvector(0,*ndata);
  sig  =(double*) dvector(1,*ndata);
  
  /*  for  (i=1;i<=*ndata;i++) printf(" xid = %9.3f yid=%9.3f lid=%9.3f\n", x[i],y[i],l[i]);*/
  xsh  =x+start_index-1;
  ysh  =y+start_index-1;
  lsh  =l+start_index-1;
  
  lista=ivector(1,ncoef);
  
  for (i=1; i<=ncoef; i++) lista[i] = i;
  for (i=1; i<=*ndata; i++) sig[i]=1.;

  SCKGETC("POLTYP", 1, 8, &actvals, poltyp);
  if (toupper(poltyp[0]) == 'L')
    lfit2_2D(x,y,l,sig,*ndata,coef,ncoef,lista,ncoef,covar,chisq,fleg_2D);
  else if (toupper(poltyp[0]) == 'C')
    lfit2_2D(x,y,l,sig,*ndata,coef,ncoef,lista,ncoef,covar,chisq,fcheb_2D);
  else if (toupper(poltyp[0]) == 'P')
    lfit2_2D(x,y,l,sig,*ndata,coef,ncoef,lista,ncoef,covar,chisq,fpoly_2D);
  else 
    printf("ERROR - You have tried an invalid polynom type \n"); 

  *chi = *chisq;
  
  free_dmatrix(covar,1,*ndata,1,*ndata);
  free_dvector(chisq,0,*ndata);
  free_dvector(sig,1,*ndata);
  free_ivector(lista,1,ncoef);
  
  pixbin = coef[2];
  return(pixbin);
  
}


/* -------------------------------------------------------------------------- */
#ifdef __STDC__
void mos_eval_disp_2D (double x[], double y[], double l[], int n)
#else
void mos_eval_disp_2D (x, y, l, n)
     double x[], y[], l[];
     int n;
#endif

{

  int actvals, i, icoef;
  double xp[MAXNCOE];
  char   poltyp[8],text[100];

  SCKGETC("POLTYP", 1, 8, &actvals, poltyp);
  for (i = start_index; i < (n + start_index); i++)
    {
      l[i] = 0.;

      if (toupper(poltyp[0]) == 'L')
        fleg_2D (x[i], y[i], xp, ncoef);
      else if (toupper(poltyp[0]) == 'C')
        fcheb_2D (x[i], y[i], xp, ncoef);
      else if (toupper(poltyp[0]) == 'P')
        fpoly_2D (x[i], y[i], xp, ncoef);
      else
	printf("ERROR - You have tried an invalid polynom type \n"); 

      for (icoef = 1; icoef <= ncoef; icoef++)
	l[i] += coef[icoef] * xp[icoef];
    }
}

/* -------------------------------------------------------------------------- */
#ifdef __STDC__
  void mos_writedisp_2D(int line, int slit, int ypix, 
		      double y, int numrow, double rms) 
#else
  void mos_writedisp_2D(line, slit, ypix, y, numrow, rms)  
  int  line, slit, ypix, numrow;
  double y, rms;
#endif
{
  int icoef,iexp;
  double coef2D[MAXNCOE], poly;
  


  /* make 1D coeficients from 2D coeficients -- last 5 depend on (y-ystart)*/
  for (icoef = 1; icoef <= (maxcoef-(degy+degxy)); icoef++)
    coef2D[icoef-1] = coef[icoef];
  poly = y;
  for (iexp = 1; iexp <= degy; iexp++)
    {
      coef2D[0] = coef2D[0] + coef[(maxcoef-degy)-degxy+iexp]*poly;
      poly = poly * y;
    }
  poly = y;
  for (iexp = 1; iexp <= degxy; iexp++)
    {
      coef2D[1] = coef2D[1] + coef[maxcoef-degxy+iexp] * poly;
      poly = poly * y;
    }
  TCEWRI(tide, line, colslit, &slit);
  TCEWRI(tide, line, colline, &ypix);
  TCEWRD(tide, line, coly,    &y);
  TCEWRD(tide, line, colrms,  &rms); 
  
  /*      if (nbline<line) nbline=line;   */
  /*if (nbline<line) nbline=numrow;*/
  
  for (icoef=1; icoef<=maxcoef-(degy+degxy); icoef++)
    TCEWRD(tide, line, colcoef[icoef], &coef2D[icoef-1]);
}

#ifdef __STDC__
  void fleg_2D( double x1, double x2, double pl[], int nl)
#else
  void fleg_2D(x1,x2,pl,nl)
  int nl;
  double x1,x2,pl[];
#endif  
{
  /* Legendre polynom in x1 - polynom in y and one mixed polynom x1*x2*/
  int j;
  float twox,f2,f1,d;
  
  pl[1]=1.0;
  pl[2]=x1;
  if (nl > 2) 
    {
      twox=2.0*x1;  /*Legendre in x*/
      f2=x1;
      d=1.0;
      for (j=3;j<=nl-(degy+degxy);j++) 
	{
	  f1=d;
	  d += 1.0;
	  f2 += twox;
	  pl[j]=(f2*pl[j-1]-f1*pl[j-2])/d;
	}

      pl[nl-(degy+degxy)+1]=x2;  /*polynom in y*/
      for (j=nl-(degy+degxy)+2;j<=(nl-degxy);j++)
	pl[j] = x2*pl[j-1];
      
      pl[nl-degxy+1] = x1*x2;    /*mixed polynom*/  
      for (j=nl-degxy+2;j<=nl;j++)
	pl[j] = x2*pl[j-1];
    }
}

#ifdef __STDC__
  void fcheb_2D( double x1, double x2, double pl[], int nl)
#else
  void fcheb_2D(x1,x2,pl,nl)
  int nl;
  double x1,x2,pl[];
#endif  
{
  /* Chebichev polynom in x1 - polynom in y and one mixed polynom x1*x2*/
  int j;
  float twox,f2,f1,d;
  
  pl[1]=1.0;
  pl[2]=x1;
  if (nl > 2) 
    {
      twox=2.0*x1;  /*Chebichev in x*/
      for (j=3;j<=nl-(degy+degxy);j++) 
	pl[j]=twox*pl[j-1]-pl[j-2];
  
      pl[nl-(degy+degxy)+1]=x2;  /*polynom in y*/
      for (j=nl-(degy+degxy)+2;j<=(nl-degxy);j++)
	pl[j] = x2*pl[j-1];
      
      pl[nl-degxy+1] = x1*x2;    /*mixed polynom*/  
      for (j=nl-degxy+2;j<=nl;j++)
	pl[j] = x2*pl[j-1];
    }
}

#ifdef __STDC__
  void fpoly_2D( double x1, double x2, double pl[], int nl)
#else
  void fpoly_2D(x1,x2,pl,nl)
  int nl;
  double x1,x2,pl[];
#endif  
{
  /* standard polynom in x1 (nl) && in x2 (degy) and 
     mixed polynom x1*x2 (degxy) */
  int j;
  float twox,f2,f1,d;
  
  pl[1]=1.0;
  if (nl > 1) 
    {
      for (j=2;j<=nl-(degy+degxy);j++)  /*polynom in x*/
	pl[j]=x1*pl[j-1];

      pl[nl-(degy+degxy)+1] = x2;  /*polynom in y*/
      for (j=nl-(degy+degxy)+2;j<=(nl-degxy);j++)
	pl[j] = x2*pl[j-1];

      pl[nl-degxy+1] = x1*x2;    /*mixed polynom*/  
      for (j=nl-degxy+2;j<=nl;j++)
	pl[j] = x2*pl[j-1];
    }
}


static float sqrarg;
#define SQR(a) (sqrarg=(a),sqrarg*sqrarg)

#ifdef _STDC__
void lfit2_2D(double x1[], double x2[], double y[], double sig[], int ndata, 
	   double a[], int ma, int lista[], int mfit, double **covar, 
	   double *chisq, void (*funcs)(double, double, double *, int))
#else
void lfit2_2D(x1, x2, y,  sig,  ndata, a,  ma,  lista,  mfit, covar, chisq,
           funcs)
double x1[],x2[],y[],sig[],a[],**covar,*chisq;
int ndata,ma,lista[],mfit;
void (*funcs) ();
#endif
{
	int k,kk,j,ihit,i;
	double ym,wt,sum,sig2i,**beta,*afunc;
	/*      void gaussj(double **, int, double **, int ); 
		void covsrt(double **, int, int [], int );
		void nrerror(char []);
		void free_dmatrix(double **, int, int, int, int);
		void free_dvector(double *, int, int);
		double **dmatrix(int, int, int, int),*dvector(int, int);
	*/
	/*      now: these are define at the top of the mosdisp2D.c library	
        */
	beta=dmatrix(1,ma,1,1);
	afunc=(double*) dvector(1,ma);
	kk=mfit+1;
	for (j=1;j<=ma;j++) {
		ihit=0;
		for (k=1;k<=mfit;k++)
			if (lista[k] == j) ihit++;
		if (ihit == 0)
			lista[kk++]=j;
		else if (ihit > 1) nrerror("Bad LISTA permutation in LFIT-1");
	}
	if (kk != (ma+1)) nrerror("Bad LISTA permutation in LFIT-2");
	for (j=1;j<=mfit;j++) {
		for (k=1;k<=mfit;k++) covar[j][k]=0.0;
		beta[j][1]=0.0;
	}
	for (i=1;i<=ndata;i++) {
		(*funcs)(x1[i],x2[i],afunc,ma);
		ym=y[i];
		if (mfit < ma)
			for (j=(mfit+1);j<=ma;j++)
				ym -= a[lista[j]]*afunc[lista[j]];
		sig2i=1.0/SQR(sig[i]);
		for (j=1;j<=mfit;j++) {
			wt=afunc[lista[j]]*sig2i;
			for (k=1;k<=j;k++)
				covar[j][k] += wt*afunc[lista[k]];
			beta[j][1] += ym*wt;
		}
	}
	if (mfit > 1)
		for (j=2;j<=mfit;j++)
			for (k=1;k<=j-1;k++)
				covar[k][j]=covar[j][k];
	gaussj(covar,mfit,beta,1);
	for (j=1;j<=mfit;j++) a[lista[j]]=beta[j][1];
	*chisq=0.0;
	for (i=1;i<=ndata;i++) {
		(*funcs)(x1[i],x2[i],afunc,ma);
		for (sum=0.0,j=1;j<=ma;j++) sum += a[j]*afunc[j];
		*chisq += SQR((y[i]-sum)/sig[i]);
	}
	covsrt(covar,ma,lista,mfit);
	free_dvector(afunc,1,ma);
	free_dmatrix(beta,1,ma,1,1);
}