#ifdef WIN
#pragma warning (disable: 4786) // disable warning for STL maps
#endif /* WIN */

/*
 * file: utils/math_utils.cpp
 * Purpose: some useful math routines not available in C - implementation
 * Author: Thomas Ott
 *
 * History: 30.06.1999: file created
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "fits.h"
#include "math_utils.h"

/* Determine nearest integer to specified double value */
int nint(double x)
{
	int k;
	
	k = (int)x;
	if (fabs(x - k) < 0.5 ) {
		return k;
	} else {
		if (x >= 0.0) return k + 1;
		else return k - 1;
	}
}

/* Fraction of a real number */
double frac(double x) {
	double rv;

	rv = x - (double)((long)x);
	if (rv < 0.0) rv++;

	return rv;
}

int floatcmp(const void *a, const void *b)
{
	if (*(float *)a < *(float *)b) return(-1);
	if (*(float *)a > *(float *)b) return(1);
	return(0);
}

/* Cosine for an argument in degrees */
double cosdeg(double x) {
	return(cos(x * M_PI / 180.0));
}

/* Sine for an argument in degrees */
double sindeg(double x) {
	return(sin(x * M_PI / 180.0));
}

/* Convert radians to degrees */
double rad2deg(double x) {
	return (x * 180.0 / M_PI);
}

/* Convert degrees to radians */
double deg2rad(double x) {
	return (x / 180.0 * M_PI);
}

/* Square of a real number */
double sqr(double x) {
	return(x*x);
}

#ifdef WIN
double asinh(double x) {
	return log(x + sqrt(x * x + 1.0));
}

double acosh(double x) {
	return log(x + sqrt(x * x - 1.0));
}

double atanh(double x) {
	return 0.5 * log((1.0 + x) / (1.0 - x));
}
#ifdef _WINDOWS // only for Visual C++ 6.0
extern "C" double rint(double x) {
	return (double)nint(x);
}
#endif
#endif /* WIN */

double trans_chi2(int ncoords, double *S, double *ref, double *xim, double *yim, double *error) {
	int i;
	double result = 0.0;
	double polynomial;
	
	for (i = 0; i < ncoords; i++) {
		polynomial = S[5] + S[0] * xim[i] + S[1] * yim[i] + S[2] * sqr(xim[i]) + S[3] * sqr(yim[i]) + S[4] * xim[i] * yim[i];
		result += sqr((ref[i] - polynomial) / error[i]);
	}
	return result;
}

/* PURPOSE: A least square fit is used in order to find 
;	   the parameters for linearly fitting a reference
;	   image to a any other image of the same star field.
;	   --> For further description and PASCAL source code
;	       see Montenbruck/Pfleger: "Astronomie mit dem 
;			Personal Computer", 1989, p. 198ff	 
;
; VARIABLES: n - Number of equations = number of reference sources
;	     m - Number of fitting parameters (3 for a linear fit, 6 for quadratic fit)
;	     A - coefficient matrix with n lines and m+1 columns
;		 structure of line k: x1 y1 1 X1
;			with (x1,y1) - coordinates of a reference star
;				       in the selected image 
;				   1 - cofficient for additive parameter
;			          X1 - x-coordinate of a reference 
;					star in the reference image 
;
; COMMENTS: Procedure has to be called two times in order to obtain all
;	    six plate constants. In the first call, the (m+1)st column
;	    of A contains the x-coordinates of the reference stars in 
; 	    the reference image; in the second call, these are replaced 
;	    by the y-coordinates.  */

double lsqfit(double **A, double *S, int n, int m) {
	double LEPS = 1.0e-10;
	double p = 0.0, q = 0.0, h = 0.0;
	double res = 0.0;
	long i = 0L, k = 0L, j = 0L;
/*	; i = 0...n (number of reference sources=number of equations) */
/*	; j = 0...m (number of parameters for linear fit) */

	for (j = 0; j < m; j++) { // Loop over columns 1..m of A
  
/* eliminate elements A[j,i] with i>j from columns j */

		for (i = (j+1); i < n; i++) {
			if ((A[j][i] > 0.0) || (A[j][i] < 0.0)) {
/* compute p, q, and new A[j,j] */
/* set A[j,i] = 0 */
				if (fabs(A[j][j]) < (LEPS * fabs(A[j][i]))) {
					p = 0.0; q = 1.0; A[j][j] = -A[j][i]; A[j][i] = 0.0;
				} else {
					h = sqrt(A[j][j] * A[j][j] + A[j][i] * A[j][i]);
					if (A[j][j] < 0.0) h = -h;
					p = A[j][j]/h; q = -A[j][i]/h; A[j][j] = h; A[j][i] = 0.0;
				}
/* Change rest of current line */
				for (k = (j+1); k <= m; k++) {
					h = p*A[k][j] - q*A[k][i];
					A[k][i] = q*A[k][j] + p*A[k][i];
					A[k][j] = h;
				}
			}
		}
	}
/* compute and print out residual error */
	for (i = m; i < n; i++) res += sqr(A[m][i]);
	res = sqrt(res);
//	printf("lsqfit: Residual error is %.10f\n", res);
	
/* compute elements of vector S */

	for (i = (m-1); i >= 0; i--) {
		h = A[m][i];
		for (k = (i+1); k < m; k++) {
			h = h + A[k][i]*S[k];
/*;	print, S[k], A[k,i], h */
		}
		S[i] = -h/A[i][i];
	}
	return res;
}

/*
 * Calculate transformation matrix between two lists of coordinates. The solution of
 * the fit is given as quadratic equations. If 1 reference coordinate is given, only
 * the shift is calculated. If 2 coordinates are given, the scale in both coordinates
 * is assumed to be the same, then this scale, the shift and rotation are calculated.
 * Given 3 to 5 coordinates, a linear equation will be calculated, using lsqfit to
 * solve the potential overdetermined set of equations. Starting with 6 coordinates,
 * a full quadratic solution will be determined.
 *
 * The transformation equations are defined as follows:
 *  x' = SX[0]*x+SX[1]*y+SX[2]*x*x+SX[3]*y*y+SX[4]*x*y+SX[5]
 *  y' = SY[0]*x+SY[1]*y+SY[2]*x*x+SY[3]*y*y+SY[4]*x*y+SY[5]
 *
 * The routine takes the following input parameters:
 *  ncoords : Number of coordinates
 *  xref    : Pointer to the reference coordinates, organised as [x0, x1, ..., x(n-1)]
 *  yref    : Pointer to the reference coordinates, organised as [y0, y1, ..., y(n-1)]
 *  xin     : Pointer to the coordinates to be fitted, organised as xref
 *  yin     : Pointer to the coordinates to be fitted, organised as yref
 *  SX      : Pointer to the solved equation for x, must be at least double[6];
 *  SY      : Pointer to the solved equation for y, must be at least double[6];
 */

int trans_matrix(int ncoords, int nused, double *xref, double *yref, double *xim, double *yim, double *SX, double *SY, double *xerror, double *yerror) {
	int i;//, j, k;
	double *A[11], angle, scale;
	int malloc_error = 0;

	for (i = 0; i < 10; i++) SX[i] = SY[i] = 0.0;
	for (i = 0; i <= 10; i++) if ((A[i] = (double *)malloc(ncoords * sizeof(double))) == NULL) malloc_error = 1;
	*xerror = *yerror = 0.0;

	if (malloc_error) {
		for (i = 0; i <= 10; i++) if (A[i] != NULL) free(A[i]);
		dp_output("trans_matrix: Memory allocation error\n");
		return 0;
	} else switch(nused) {
		case 1:
			SX[5] = xref[0] - xim[0];
			SY[5] = yref[0] - yim[0];
			SX[0] = SY[1] = 1.0;
			break;
		case 2:
/*			nenner = (xref[0] - xref[1]) * (xref[0] - xref[1]) + (yref[0] - yref[1]) * (yref[0] - yref[1]);
			if (nenner != 0.0) {
				SX[0] = ((xref[0] - xref[1]) * (xim[0] - xim[1]) + (yref[0] - yref[1]) * (yim[0] - yim[1])) / nenner;
				SY[1] = SX[0];
				SX[1] = ((yref[0] - yref[1]) * (xim[1] - xim[0]) + (xref[0] - xref[1]) * (yim[0] - yim[1])) / nenner;
				SY[0] = SX[1];
				SX[1] = -SX[1];
				SX[5] = (xref[0] * xref[0] * xim[1]
				      +  xim[0] * (xref[1] * xref[1] - yref[0] * yref[1] + yref[1] * yref[1])
				      +  yref[0] * (xim[1] * yref[0] - xref[1] * yim[0] - xim[1] * yref[1] + xref[1] * yim[1])
							-  xref[0] * (xim[0] * xref[1] + xref[1] * xim[1] - yim[0] * yref[1] + yref[1] * yim[1]))
							/ nenner;
				SY[5] = (xref[1] * xref[1] * yim[0]
				      +  xref[0] * xim[1] * yref[1]
							-  yref[0] * yim[0] * yref[1]
							+  yim[0] * yref[1] * yref[1]
							+  xim[0] * xim[0] * (xref[1] * yref[0] - xref[0] * yref[1])
							+  xref[0] * xref[0] * yim[1]
							+  yref[0] * yref[0] * yim[1]
							-  yref[0] * yref[1] * yim[1]
							-  xref[1] * (xim[1] * yref[0] + xref[0] * yim[0] + xref[0] * yim[1]))
							/  nenner;
			}*/

/* rainer */
/*     nenner = (xref[0] - xref[1]) * (xref[0] - xref[1]) + (yref[0] - yref[1]) * (yref[0] - yref[1]);
		 printf("%.10f\n", nenner);
			if (nenner != 0.0) {
					SY[0] = ((xref[0] - xref[1]) * (yim[0] - yim[1]) - (yref[0] - yref[1]) * (xim[0] - xim[1])) / nenner;
		      SX[1] = SY[0];
					SY[0] = -SY[0];
					SY[1] = (xim[0] - xim[1])/(xref[0] - xref[1]) + SY[1] * (yref[0] - yref[1])/(xref[0] - xref[1]);
			    SX[0] = SY[1];
          SX[5] = xim[0] - SX[0] * xref[0] - SX[1] * yref[0];
					SY[5] = yim[0] - SY[0] * xref[0] - SY[1] * yref[0];
					scale = sqrt(SX[0] * SX[0] + SY[0] * SY[0]);
					SX[5] *= fabs(SX[0]) / scale;
					SY[5] *= fabs(SX[0]) / scale;
      }*/
			
/* end rainer */
				angle = atan2((yim[0] - yim[1]), (xim[0] - xim[1]));
				angle -= atan2((yref[0] - yref[1]), (xref[0] - xref[1]));
				scale = sqr(yref[0] - yref[1]) + sqr(xref[0] - xref[1]);
				scale /= sqr(yim[0] - yim[1]) + sqr(xim[0] - xim[1]);
				scale = sqrt(scale);
				scale = 1. / scale;
				dp_debug("angle = %.10f\n", angle * 180. / M_PI);
				dp_debug("scale = %.10f\n", scale);
				SX[5] = xim[0] - scale * (xref[0] * cos(angle) - yref[0] * sin(angle));
				SY[5] = yim[0] - scale * (xref[0] * sin(angle) + yref[0] * cos(angle));
				SX[0] = SY[1] = scale * cos(angle);
				SY[0] = scale * sin(angle);
				SX[1] = -scale * sin(angle);
			break;
		case 3:
		case 4:
		case 5:
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = 1.0;
				A[3][i] = -xim[i];
			}
			*xerror = lsqfit(A, SX, ncoords, 3);
			SX[5] = SX[2];
			SX[2] = 0.0;
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = 1.0;
				A[3][i] = -yim[i];
			}
			*yerror = lsqfit(A, SY, ncoords, 3);
			SY[5] = SY[2];
			SY[2] = 0.0;
			break;
		case 10:
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = xref[i]*xref[i]*xref[i];
				A[6][i] = yref[i]*yref[i]*yref[i];
				A[7][i] = xref[i]*xref[i]*yref[i];
				A[8][i] = yref[i]*yref[i]*xref[i];
				A[9][i] = 1.0;
				A[10][i] = -xim[i];
			}
			*xerror = lsqfit(A, SX, ncoords, 10);
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = xref[i]*xref[i]*xref[i];
				A[6][i] = yref[i]*yref[i]*yref[i];
				A[7][i] = xref[i]*xref[i]*yref[i];
				A[8][i] = yref[i]*yref[i]*xref[i];
				A[9][i] = 1.0;
				A[10][i] = -yim[i];
			}
			*yerror = lsqfit(A, SY, ncoords, 10);
			break;
		default:
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = 1.0;
				A[6][i] = -xim[i];
			}
			*xerror = lsqfit(A, SX, ncoords, 6);
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = 1.0;
				A[6][i] = -yim[i];
			}
			*yerror = lsqfit(A, SY, ncoords, 6);
			break;
		}
	
	for (i = 0; i <= 10; i++) if (A[i] != NULL) free(A[i]);
	return 1;
}

int trans_matrix_errors(int ncoords, int nused, double *xref, double *yref, double *xim, double *yim, double *SX, double *SY, double *xerror, double *yerror) {
	int i, j, k;
	double *A[11], angle, scale, error;
	int malloc_error = 0;

	for (i = 0; i < 10; i++) SX[i] = SY[i] = 0.0;
	for (i = 0; i <= 10; i++) if ((A[i] = (double *)malloc(ncoords * sizeof(double))) == NULL) malloc_error = 1;

	if (malloc_error) {
		for (i = 0; i <= 10; i++) if (A[i] != NULL) free(A[i]);
		dp_output("trans_matrix: Memory allocation error\n");
		return 0;
	} else switch(nused) {
		case 1:
			SX[5] = xref[0] - xim[0];
			SY[5] = yref[0] - yim[0];
			SX[0] = SY[1] = 1.0;
			break;
		case 2:
				angle = atan2((yim[0] - yim[1]), (xim[0] - xim[1]));
				angle -= atan2((yref[0] - yref[1]), (xref[0] - xref[1]));
				scale = sqr(yref[0] - yref[1]) + sqr(xref[0] - xref[1]);
				scale /= sqr(yim[0] - yim[1]) + sqr(xim[0] - xim[1]);
				scale = sqrt(scale);
				scale = 1. / scale;
				dp_debug("angle = %.10f\n", angle * 180. / M_PI);
				dp_debug("scale = %.10f\n", scale);
				SX[5] = xim[0] - scale * (xref[0] * cos(angle) - yref[0] * sin(angle));
				SY[5] = yim[0] - scale * (xref[0] * sin(angle) + yref[0] * cos(angle));
				SX[0] = SY[1] = scale * cos(angle);
				SY[0] = scale * sin(angle);
				SX[1] = -scale * sin(angle);
			break;
		case 3:
		case 4:
		case 5:
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = 1.0;
				A[3][i] = -xim[i];
			}
			error = lsqfit(A, SX, ncoords, 3);
			SX[5] = SX[2];
			SX[2] = 0.0;
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = 1.0;
				A[3][i] = -yim[i];
			}
			error = lsqfit(A, SY, ncoords, 3);
			SY[5] = SY[2];
			SY[2] = 0.0;
			break;
		case 10:
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = xref[i]*xref[i]*xref[i];
				A[6][i] = yref[i]*yref[i]*yref[i];
				A[7][i] = xref[i]*xref[i]*yref[i];
				A[8][i] = yref[i]*yref[i]*xref[i];
				A[9][i] = 1.0;
				A[10][i] = -xim[i];
			}
			error = lsqfit(A, SX, ncoords, 10);
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = xref[i]*xref[i]*xref[i];
				A[6][i] = yref[i]*yref[i]*yref[i];
				A[7][i] = xref[i]*xref[i]*yref[i];
				A[8][i] = yref[i]*yref[i]*xref[i];
				A[9][i] = 1.0;
				A[10][i] = -yim[i];
			}
			error = lsqfit(A, SY, ncoords, 10);
			break;
		default:
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = 1.0;
				A[6][i] = -xim[i];
			}
			error = lsqfit(A, SX, ncoords, 6);
			for (i = 0; i < ncoords; i++) {
				A[0][i] = xref[i];
				A[1][i] = yref[i];
				A[2][i] = xref[i]*xref[i];
				A[3][i] = yref[i]*yref[i];
				A[4][i] = xref[i]*yref[i];
				A[5][i] = 1.0;
				A[6][i] = -yim[i];
			}
			error = lsqfit(A, SY, ncoords, 6);
			break;
		}

	if (nused == 6) {
		double Sip[6], Sim[6], Sipjp[6], Sipjm[6], Simjp[6], Simjm[6];
		double eps = 0.001;
		double matrix[6][6];
		double chisq0;
		Fits imatrix;
		imatrix.create(6, 6, R8);

/* First, X coordinate */		
		chisq0 = trans_chi2(ncoords, SX, xref, xim, yim, xerror);
		for (i = 0; i < nused; i++) {
			for (k = 0; k < 6; k++) { Sip[k] = Sim[k] = SX[k]; }
			double epsi = eps * SX[i];
			Sip[i] += epsi;
			Sim[i] -= epsi;
			for (j = 0; j < nused; j++) {
				if (j < i) {
					matrix[i][j] = matrix[j][i];
				} else if (i == j) {
					matrix[i][i] = (trans_chi2(ncoords, Sip, xref, xim, yim, xerror)
								 + trans_chi2(ncoords, Sim, xref, xim, yim, xerror)
								 - 2. * chisq0) / sqr(epsi);
				} else {
					double epsj = eps * SX[j];
					for (k = 0; k < 6; k++) {
						Sipjp[k] = Sipjm[k] = Sip[k];
						Simjm[k] = Simjp[k] = Sim[k];
					}
					Sipjp[j] += epsj;
					Sipjm[j] -= epsj;
					Simjp[j] += epsj;
					Simjm[j] -= epsj;
					matrix[i][j] = (trans_chi2(ncoords, Sipjp, xref, xim, yim, xerror)
								 + trans_chi2(ncoords, Simjm, xref, xim, yim, xerror)
								 - trans_chi2(ncoords, Sipjm, xref, xim, yim, xerror)
								 - trans_chi2(ncoords, Simjp, xref, xim, yim, xerror)) / (4. * epsi * epsj);
								 
				}
			}
		}
		for (i = 0; i < 6; i++) {
			for (j = 0; j < 6; j++) {
				imatrix.r8data[imatrix.C_I(i, j)] = matrix[i][j];
			}
		}
		imatrix.div(2.);
		imatrix.ginvert();
		imatrix.Abs();
		imatrix.Sqrt();
		
		for (i = 0; i < 6; i++) xerror[i] = imatrix(i+1,i+1);

/* Second, Y coordinate */		
		chisq0 = trans_chi2(ncoords, SY, yref, xim, yim, yerror);
		for (i = 0; i < nused; i++) {
			for (k = 0; k < 6; k++) { Sip[k] = Sim[k] = SY[k]; }
			double epsi = eps * SY[i];
			Sip[i] += epsi;
			Sim[i] -= epsi;
			for (j = 0; j < nused; j++) {
				if (j < i) {
					matrix[i][j] = matrix[j][i];
				} else if (i == j) {
					matrix[i][i] = (trans_chi2(ncoords, Sip, yref, xim, yim, yerror)
								 + trans_chi2(ncoords, Sim, yref, xim, yim, yerror)
								 - 2. * chisq0) / sqr(epsi);
				} else {
					double epsj = eps * SY[j];
					for (k = 0; k < 6; k++) {
						Sipjp[k] = Sipjm[k] = Sip[k];
						Simjm[k] = Simjp[k] = Sim[k];
					}
					Sipjp[j] += epsj;
					Sipjm[j] -= epsj;
					Simjp[j] += epsj;
					Simjm[j] -= epsj;
					matrix[i][j] = (trans_chi2(ncoords, Sipjp, yref, xim, yim, yerror)
								 + trans_chi2(ncoords, Simjm, yref, xim, yim, yerror)
								 - trans_chi2(ncoords, Sipjm, yref, xim, yim, yerror)
								 - trans_chi2(ncoords, Simjp, yref, xim, yim, yerror)) / (4. * epsi * epsj);
								 
				}
			}
		}
		for (i = 0; i < 6; i++) {
			for (j = 0; j < 6; j++) {
				imatrix.r8data[imatrix.C_I(i, j)] = matrix[i][j];
			}
		}
		imatrix.div(2.);
		imatrix.ginvert();
		imatrix.Abs();
		imatrix.Sqrt();
		for (i = 0; i < 6; i++) yerror[i] = imatrix(i+1,i+1);
	}
				
				
	
	for (i = 0; i <= 10; i++) if (A[i] != NULL) free(A[i]);
	return 1;
}

// the sinc function: sinc(x) = sin(x) / x
// For arguments -0.1 < x < 0.1, return the taylor expansion
double sinc(double x) {
	if (fabs(x) > 0.1) return sin(x) / x;
	else return x*x*x*x / 120. - x*x / 6. + 1.;
}
// the cosc function: derivative of the sinc(x) = sin(x)/x
// For arguments -0.1 < x < 0.1, return the taylor expansion
double cosc(double x) {
	if (fabs(x) > 0.1) return cos(x) / x - sin(x) / x / x;
	else return -x*x*x*x*x / 840. + x*x*x / 30. - x / 3.;
}