// mathmatical functions not included in std-math-library
// 1999 Claus A. Goessl
// special version for STISPACKAGE v2.5
// 2002 Anastasia Alexov for IPMG Project

#include "cagmath.h"

//extern "C"
//{
//#include <fitsio.h>
//}

//int calcmedian(int *dataarray, long length);
/*float calcmedian(float *dataarray, long length);*/
/*double calcmedian(double *dataarray, long length);*/

//void calcminmax(const int *dataarray, const long length, int &minimum, int &maximum);
//void calcminmax(const float *dataarray, const long length, float &minimum, float &maximum);
//void calcminmax(const double *dataarray, const long length, double &minimum, double &maximum);

//double gaussposampfit(float *imagearray, const int xlen, const int ylen,
//		      double &xpos, double &ypos, double &amplitude,
//		      int xbox, int ybox, const double gain);
//int mrqmin7(double **fun, const int xbox, const int ybox, const double gain,
//	   double *a, double &chisq);
//double mrqcof7(double **fun, const int xbox, const int ybox, const double gain,
//      double *a, double **alpha, double *beta);
//int gaussj7(double ** a, double * b);
//int gaussj(double ** a, double * b, const int n);

// find median of int array
// really fast! but needs huge amount of memory: (maximum - minimum) bytes
int calcmedian(const int *dataarray, long length){
  // find min, max
  int minimum,maximum;

  calcminmax(dataarray, length, minimum, maximum);

  // build histogramm
  long *medarray;
  medarray=new long [maximum-minimum];
  if(medarray==NULL){
    cerr<<"Insufficient memory.\n";exit(1);
  }
  medarray-=minimum;
  for(int i=minimum; i<maximum; medarray[i++]=0); // clear array
  for(long j=0; j<length; j++)++medarray[dataarray[j]]; // build histogramm
  length/=2;
  maximum=minimum;
  for(long j=0; j<length; j+=medarray[maximum++]); // find half value
  medarray+=minimum;
  delete [] medarray;
  //free(medarray);
  return (maximum-1);
}

// return minimum, maximum in approbiate variables
void calcminmax(const int *dataarray, const long length, int &minimum, int &maximum){
  minimum=dataarray[0];
  maximum=dataarray[0];
  for(long j=1; j<length; j++){
    if(dataarray[j]>maximum)maximum=dataarray[j];
    else if(dataarray[j]<minimum)minimum=dataarray[j];
  }
}
void calcminmax(const float *dataarray, const long length, float &minimum, float &maximum){
  minimum=dataarray[0];
  maximum=dataarray[0];
  for(long j=1; j<length; j++){
    if(dataarray[j]>maximum)maximum=dataarray[j];
    else if(dataarray[j]<minimum)minimum=dataarray[j];
  }
}
void calcminmax(const double *dataarray, const long length, double &minimum, double &maximum){
  minimum=dataarray[0];
  maximum=dataarray[0];
  for(long j=1; j<length; j++){
    if(dataarray[j]>maximum)maximum=dataarray[j];
    else if(dataarray[j]<minimum)minimum=dataarray[j];
  }
}

// fit position and amplitude of a 2-dim rotated gaussian
// return fiterror: average stddev in each pixel
double gaussposampfit(float *imagearray, const int xlen, const int ylen,
		      double &xpos, double &ypos, double &amplitude,
		      int xbox, int ybox, const double gain){
  // check startpoint if inside image
  if((xpos<1.0)||(xpos>(double)xlen)||(ypos<1.0)||(ypos>(double)ylen))return -1;

  // check imageblock, if inside image
  // if not, trim block
  int xstart=(int)xpos-(xbox/2);
  if(xstart<0){xstart=0;xbox/=2;xbox+=(int)xpos;}
  else if(xstart+xbox>=xlen)xbox=xlen-xstart-1;
  int ystart=(int)ypos-(ybox/2);
  if(ystart<0){ystart=0;ybox/=2;ybox+=(int)ypos;}
  else if(ystart+ybox>=ylen)ybox=ylen-ystart-1;
  // check on minimal blocksize 5 x 5
  if((xbox<5)||(ybox<5))return -1;

  // copy imageblock in local array
  double **fun;
  if((fun=new double * [ybox])==NULL)return -2;
  if((fun[0]=new double [xbox*ybox])==NULL){delete [] fun;return -2;}
  for(int i=1, j=xbox; i<ybox;i++,j+=xbox)fun[i]=fun[0]+j;
  for(int y=0; y<ybox; y++){
    float *dataptr=imagearray+((ystart+y)*xlen)+xstart;
    for(int x=0; x<xbox; x++, dataptr++)
      fun[y][x]=(double)(*dataptr);
  }
  double a[7]; // array for fitparameters
  // !!! You may alter these start values
  // they have worked fine for my test spectrum
  a[0]=0; // start amplitude, see below
  a[1]=4.0*log(2.0)/((xbox*xbox)/9.0); // xwidth
  a[2]=4.0*log(2.0)/((ybox*ybox)/9.0); // ywidth
  // start coordinates, alternate version see below
  a[3]=xpos-(double)xstart;a[4]=ypos-(double)ystart; // x-, y-position
  a[5]=175.0*M_PI/180.0; // start angle, should not be zero
  a[6]=fun[0][0]; // constant surface, see below
  // get start parameters amplitude = maximum, 
  // x = x_max, y = y_max, constant = minimum
  for(int y=0; y<ybox; y++)for(int x=0; x<xbox; x++){
    if(fun[y][x]>a[0]){
      a[0]=fun[y][x];
      // alternate version for start coordinates
      //a[3]=(double)x; 
      //a[4]=(double)y;
    }
    else if(fun[y][x]<a[6])a[6]=fun[y][x];
  }
  // alternate version
  //calcminmax(fun[0],(long)(xbox*ybox),a[6],a[0]);
  a[0]-=a[6];

  double chisq=0.0;

  // run marquard-algorythm, estimate error with chisquare
  if(mrqmin7(fun, xbox, ybox, gain, a, chisq)<0)amplitude=0.0;
  else amplitude=a[0];
  xpos=((double)xstart)+a[3];
  ypos=((double)ystart)+a[4];
  chisq/=(xbox*ybox);
  if(chisq>amplitude)chisq=0.0;
  else chisq/=amplitude;

  delete [] fun[0];
  delete [] fun;
  return chisq;
}

// 2-dim rotated gaussfit
// marquart-algorithm for 7 parameters
// original algorithm of numerical recipes
// see additional information and original routine in
// NUMERICAL RECIPES: Marquart-algorithm to fit values with a model function
int mrqmin7(double **fun, const int xbox, const int ybox, const double gain,
	   double *a, double &chisq){

  const int n=7; // no of fitparameters
  const int iteration=1000; // maximum no of iterations
  const double alamdastart=0.001; // initial value of alamda
  const double alamdastep=5.0; // modification factor for alamda
  const double alamdamax=1.0e10; // maximal alamda
  const double alamdamin=1.0e-10; // minimal alamda

  double **alpha, **covar;
  double *atry, *da, *beta;

  // allocate memory
  if((alpha=new double * [2*n])==NULL)return -2;
  covar=alpha+n;
  if((atry=new double [(3*n)+(2*n*n)])==NULL){delete [] alpha; return -2;};
  da=atry+n;beta=da+n;
  alpha[0]=beta+n;
  for(int i=1; i<(2*n); i++)alpha[i]=alpha[i-1]+n;

  // initialize matrices and vectors etc.
  double alamda=alamdastart;
  chisq=mrqcof7(fun, xbox, ybox, gain, a, alpha, beta);
  double ochisq=chisq;
  for(int i=0; i<n; i++)atry[i]=a[i];

  // now run marquart-algorithm
  int i=0;
  while((i<iteration)&&(alamda>alamdamin)&&(alamda<alamdamax)){
    // build testarrays
    for(int j=0; j<n; j++){
      for(int k=0; k<n; k++)covar[j][k]=alpha[j][k];
      covar[j][j]*=1.0+alamda;
      da[j]=beta[j];
    }
    // solve matrix equation
    if(gaussj7(covar,da)){delete [] atry;delete [] alpha;return -1;}
    for(int j=0; j<n; j++)atry[j]=a[j]+da[j];
    // test function
    chisq=mrqcof7(fun, xbox, ybox, gain, atry, covar, da);
    // better?
    if(chisq<ochisq){
      alamda/=alamdastep;
      for(int j=0; j<n; j++){
	for(int k=0; k<n; k++)alpha[j][k]=covar[j][k];
	beta[j]=da[j];
	a[j]=atry[j];
      }
      ochisq=chisq;
    }
    // worse!
    else{
      chisq=ochisq;
      alamda*=alamdastep;
    }
    ++i;
  }
  delete [] atry;
  delete [] alpha;
  return i;
}

// determine new coefficients for the marquart-algorithm
// version for a 2-dim-rotated-gaussian
// fitparameters: Amplitude, x-, y-Width, x-, y-Position, rotation angle
// fun is dataarray, size in x and ybox, 
// gain is conversion factor to determine physical errors and weight them
// set zero for no weighting
// new coefficients are some way of the old ones and estimated by deviations
// of the fit function by trying to minimize the difference of function and values
// see additional information and original routine in
// NUMERICAL RECIPES: Marquart-algorithm to fit values with a model function

double mrqcof7(double **fun, const int xbox, const int ybox, const double gain,
       double *a, double **alpha, double *beta){

  const int n=7; // no of fitparameters
  double dfda[7];
  //double *dfda;
  //if((dfda=new double [n])==NULL)return 0.0;

  for(int l=0; l<n; l++){
    for(int k=0; k<=l; k++)alpha[l][k]=0.0;
    beta[l]=0.0;
    dfda[l]=0.0;
  }
  double chisq=0.0;

  double am=a[0]; // amplitde
  double bx=a[1]; // xwidth
  double by=a[2]; // ywidth
  double x0=a[3]; // xposition
  double y0=a[4]; // yposition
  double cw=cos(a[5]); // cos of angle
  double sw=sin(a[5]); // sin of angle
  for(int yy=0; yy<ybox; yy++){
    double ydif=((double) yy)-y0;
    for(int xx=0; xx<xbox; xx++){
      if(fun[yy][xx]){
	double xdif=((double) xx)-x0;
	double t1=xdif*cw+ydif*sw;
	double t2=ydif*cw-xdif*sw;
	double t1s=t1*t1;
	double t2s=t2*t2;
	double ert=-(bx*t1s+by*t2s);
	if(ert>22)ert=22;
	else if(ert<-22)ert=-22;
	ert=exp(ert);
	double yg=am*ert;
	dfda[0]=ert;
	dfda[1]=-(t1s*yg);
	dfda[2]=-(t2s*yg);
	dfda[3]=2.0*(bx*t1*cw-by*t2*sw)*yg;
	dfda[4]=2.0*(bx*t1*sw+by*t2*cw)*yg;
	dfda[5]=2.0*(by-bx)*t1*t2*yg;
	dfda[6]=1.0;
	double df=fun[yy][xx];
	double sig2i;
	if((gain)&&(df>0))sig2i=sqrt(gain/df);
	else sig2i=1.0;
	df-=yg+a[6];
	for(int l=0; l<n; l++){
	  double wt=dfda[l]*sig2i;
	  for(int k=0; k<=l; k++)alpha[l][k]+=wt*dfda[k];
	  beta[l]+=df*wt;
	}
	chisq+=df*df*sig2i;
      }
    }
  }
  for(int l=1; l<n; l++)for(int k=0; k<l; k++)alpha[k][l]=alpha[l][k];

  return chisq;
}

// solve matrix equation, gauss-jordan
// a speed up version for 7 elements
// 
// see additional information and original routine 
// below in gaussj() and in
// NUMERICAL RECIPES: Gauss-Jordan method to solve a matrix
int gaussj7(double ** a, double * b){
  // double a[n][n]; double b[n]; int n;
  // get local vectors
  // free n, dimension
  //int *indxc, *indxr, *ipiv;
  //if((indxc=new int [3*n])==NULL)return -2;
  //indxr=indxc+n;ipiv=indxr+n;
  // fixed n, dimension
#define N_VECTOR_SIZE 7
  const int n=N_VECTOR_SIZE;
  int indxc[N_VECTOR_SIZE], indxr[N_VECTOR_SIZE], ipiv[N_VECTOR_SIZE];

  int i;
  for(i=0; i<n; i++)ipiv[i]=0; // clear Pivot
  for(i=0; i<n; i++){
    int icol=0, irow=0;
    double big=0;
    for(int j=0; j<n; j++){
      if(ipiv[j]!=1){
	for(int k=0; k<n; k++){
	  if(ipiv[k]==0){
	    if(fabs(a[j][k])>=big){
	      big=fabs(a[j][k]);
	      irow=j;
	      icol=k;
	    }
	  }
	  else if(ipiv[k]>1)return -1;
	}
      }
    }
    ++ipiv[icol];
    if(irow!=icol){
      // swap matrix lines and vector elemants
      for(int l=0; l<n; l++){
	register double dum=a[irow][l];
	a[irow][l]=a[icol][l];
	a[icol][l]=dum;
      }
      register double dum=b[irow];
      b[irow]=b[icol];
      b[icol]=dum;
    }
    indxr[i]=irow;
    indxc[i]=icol;
    if(a[icol][icol]==0)return -1;
    double pivinv=1.0/a[icol][icol];
    a[icol][icol]=1.0;
    for(int l=0; l<n; l++)a[icol][l]*=pivinv;
    b[icol]*=pivinv;
    for(int ll=0; ll<n; ll++){
      if(ll!=icol){
	register double dum=a[ll][icol];
	a[ll][icol]=0.0;
	for(int l=0; l<n; l++)a[ll][l]-=a[icol][l]*dum;
	b[ll]-=b[icol]*dum;
      }
    }
  }
  for(int l=n-1; l>=0; l--){
    if(indxr[l]!=indxc[l]){
      for(int k=0; k<n; k++){
	register double dum=a[k][indxr[l]];
	a[k][indxr[l]]=a[k][indxc[l]];
	a[k][indxc[l]]=dum;
      }
    }
  }
  return 0;
}

// solve matrix equation, gauss-jordan
// this is a version with free dimensions
// to speed it up select a fixed n and substitute
// the memory allocation of the local vectors
// you can see this above in gaussj7()
// see additional information and original routine in
// NUMERICAL RECIPES: Gauss-Jordan method to solve a matrix
int gaussj(double ** a, double * b, const int n){
  // double a[n][n]; double b[n]; int n;
  // get local vectors
  // free n, dimension
  int *indxc, *indxr, *ipiv;
  if((indxc=new int [3*n])==NULL)return -2;
  indxr=indxc+n;ipiv=indxr+n;
  // fixed n, dimension
  // int indxc[n], indxr[n], ipiv[n];

  {for(int i=0; i<n; i++)ipiv[i]=0;} // clear Pivot
  for(int i=0; i<n; i++){
    int icol=0, irow=0;
    double big=0;
    for(int j=0; j<n; j++){
      if(ipiv[j]!=1){
	for(int k=0; k<n; k++){
	  if(ipiv[k]==0){
	    if(fabs(a[j][k])>=big){
	      big=fabs(a[j][k]);
	      irow=j;
	      icol=k;
	    }
	  }
	  else if(ipiv[k]>1){delete [] indxc;return -1;}
	}
      }
    }
    ++ipiv[icol];
    if(irow!=icol){
      // swap matrix lines and vector elemants
      for(int l=0; l<n; l++){
	register double dum=a[irow][l];
	a[irow][l]=a[icol][l];
	a[icol][l]=dum;
      }
      register double dum=b[irow];
      b[irow]=b[icol];
      b[icol]=dum;
    }
    indxr[i]=irow;
    indxc[i]=icol;
    if(a[icol][icol]==0){delete [] indxc;return -1;}
    double pivinv=1.0/a[icol][icol];
    a[icol][icol]=1.0;
    for(int l=0; l<n; l++)a[icol][l]*=pivinv;
    b[icol]*=pivinv;
    for(int ll=0; ll<n; ll++){
      if(ll!=icol){
	register double dum=a[ll][icol];
	a[ll][icol]=0.0;
	for(int l=0; l<n; l++)a[ll][l]-=a[icol][l]*dum;
	b[ll]-=b[icol]*dum;
      }
    }
  }
  for(int l=n-1; l>=0; l--){
    if(indxr[l]!=indxc[l]){
      for(int k=0; k<n; k++){
	register double dum=a[k][indxr[l]];
	a[k][indxr[l]]=a[k][indxc[l]];
	a[k][indxc[l]]=dum;
      }
    }
  }
  delete [] indxc;
  return 0;
}

double fastsqrt(double a, const int niter){
  int i;
  double x=(a+1.0)*0.5;
  for(i=0; i<niter; i++)
    x=((a/x)+x)*0.5;
  return x;
}