/**************************************************************

matsolve.c (C-Munipack project)
Copyright (C) 2003 David Motl, dmotl@volny.cz

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.

**************************************************************/
#define _USE_MATH_DEFINES

#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <assert.h>

#include "comfun.h"
#include "matfun.h"
#include "match.h"
#include "fft.h"

#define FLOAT_MAGIC   (-((float)  0x80000000) * ((float)  0x80000000))

/* machine epsilon real number (appropriately) */
#define MACHEPS 1.0E-15

/* Transformation matrix mapping */
#define M(x,y) m[(x)+(y)*3]

static double mexicanfilterfunction(double ki, double kj, double s12, double s22)
{
	double	kk = ki * ki + kj * kj;
	return (exp(-0.5 * kk * s12) - exp(-0.5 * kk * s22));
}

static void	substitute(double **f, int Nx, int Ny, double magicsubstitute)
{
	int	x, y;
	for (y = 0; y < Ny; y++) {
		for (x = 0; x < Nx; x++) {
			if (f[y][x] == (double) FLOAT_MAGIC)
				f[y][x] = magicsubstitute;
		}
	}
}

static void filter(fft_type fk, int Nx, int Ny, double s12, double s22, 
	double (*filterfunc)(double ki, double kj, double s12, double s22))
{
	int	ix, iy, n;
	double	T, kx, ky;
	complex **Fk;

	Fk = (complex **) fk;
	n = Ny / 2;
	for (iy = 0; iy <= n; iy++) {
		ky = 2 * M_PI * iy / (double) Ny;
		for (ix = 0; ix < Nx; ix++) {
			kx = (ix < Nx / 2 ? ix : ix - Nx) * 2 * M_PI / (double) Nx;
			T = filterfunc(kx, ky, s12, s22);
			Fk[iy][ix].r *= T;
			Fk[iy][ix].i *= T;
		}
	}
}

static void	mexicanfilter(double **f, 
					int N1, int N2,
					double **fs, 
					double sigma1, 
					double sigma2,
					double magicsubstitute)
{
	fft_type 	fk;

	substitute(f, N1, N2, magicsubstitute);
	alloc_fft(&fk, N1, N2);
	forward_fft(f, N1, N2, fk);
	filter(fk, N1, N2, sigma1*sigma1, sigma2*sigma2, mexicanfilterfunction); 
	inverse_fft(fk, N1, N2, fs);
	free_fft(fk, N1, N2);
}

static int allocFloatArray(double ***f, int N1, int N2)
{
	int		i;
	
	(*f) = (double **) cmpack_calloc(N2, sizeof(double *));
	if (!(*f)) {
		return CMPACK_ERR_MEMORY;
	}
	(*f)[0] = (double *) cmpack_calloc(N1 * N2, sizeof(double));
	if (!(*f)[0]) {
		cmpack_free(*f);
		(*f) = NULL;
		return CMPACK_ERR_MEMORY;
	}
	for (i = 1; i < N2; i++)
		(*f)[i] = (*f)[i - 1] + N1;
	return 0;
}

static void freeFloatArray(double **f, int N1, int N2)
{
	cmpack_free(f[0]);
	cmpack_free(f);
}
 
static int findpeak(double **f, int imsize, double *xpeak, double *ypeak, double *thepeakval)
{
	int	i, j, i0, j0, ixpeak, iypeak;	
	double	Fp, Fpp, peakval = -1.e20;
	static double **ff = NULL;

	/* allocate space for 3*3 array around peak if necessary */
	if (!ff) {
		ff = (double **) cmpack_calloc(3, sizeof(double *)) + 1;
		for (i0 = -1; i0 <= 1; i0++) {
			ff[i0] = (double *) cmpack_calloc(3, sizeof(double)) + 1;
		}
	}

	/* find hottest pixel */
	ixpeak = iypeak = -1;
	for (i = 0; i < imsize; i++) {
		for (j = 0; j < imsize; j++) {
			if (f[i][j] > peakval) {
				iypeak = i;
				ixpeak = j;
				peakval = f[i][j];
			} 
		}
	}
	*xpeak = (double) ixpeak;
	*ypeak = (double) iypeak;
	
	/* copy 3 x 3 array */
	for (i0 = -1; i0 <= 1; i0++) {
		i = iypeak + i0;
		if (i < 0) {
			i += imsize;
		}
		if (i >= imsize) {
			i -= imsize;
		}
		for (j0 = -1; j0 <= 1; j0++) {
			j = ixpeak + j0;
			if (j < 0) {
				j += imsize;
			}
			if (j >= imsize) {
				j -= imsize;
			}
			ff[i0][j0] = f[i][j];
		}
	}
	
	/* now we add the simple cludge to get more precise position */
	Fp  = 0.5 * (ff[0][1] - ff[0][-1]);
	Fpp = ff[0][1] - 2 * ff[0][0] + ff[0][-1];
	if (fabs(Fpp) > 1.e-20)
		*xpeak -= Fp / Fpp;
	Fp  = 0.5 * (ff[1][0] - ff[-1][0]);
	Fpp = ff[1][0] - 2 * ff[0][0] + ff[-1][0];
	if (fabs(Fpp) > 1.e-20)
		*ypeak -= Fp / Fpp;
	*thepeakval = peakval;

	return ixpeak>=0 && iypeak>=0;
}

static int compare_fn(const void *a, const void *b)
{
	double ma = *((double*)a), mb = *((double*)b);
	return (ma < mb ? -1 : (ma > mb ? 1 : 0));
}

static double median(int length, double *data)
{
	if (length>1) {
		if (length>2) {
			if (length&1) {
				/* Odd length */
				qsort(data, length, sizeof(double), compare_fn);
				return data[length/2];
			} else {
				/* Even length */
				qsort(data, length, sizeof(double), compare_fn);
				return 0.5 * (data[(length-1)/2] + data[(length+1)/2]);
			}
		} else {
			/* Two items */
			return 0.5 * (data[0] + data[1]);
		}
	} else {
		/* One item */
		return data[0];
	}
}

static double **allocDouble2DArray(int width, int height)
{
	int obj;
	double **retval = (double **) cmpack_calloc(height, sizeof(double *));
	if (retval) {
		for (obj = 0; obj < height; obj++) 
			retval[obj] = (double *) cmpack_calloc(width, sizeof(double));
	}
	return retval;
}

static void freeDouble2DArray(double **buf, int width, int height)
{
	if (buf) {
		int obj;
		for (obj = 0; obj < height; obj++)
			cmpack_free(buf[obj]);
		cmpack_free(buf);
	}
}

static int correlate(CmpackMatch *cfg, double *xpeak, double *ypeak, double *peakval)
{
	static const double sigma1 = 1, sigma2 = 2;

	double ***n = cfg->fft_n, **theccf = cfg->fft_theccf;
	fft_type *fk = cfg->fft_fk;
	int imsize = cfg->fft_imsize;
	
	/* do the crosscorrelation */
	forward_fft(n[0], imsize, imsize, fk[0]);
	forward_fft(n[1], imsize, imsize, fk[1]);
	ccf(fk[0], fk[1], imsize, imsize, theccf, imsize / 2, imsize / 2);
	mexicanfilter(theccf, imsize, imsize, theccf, sigma1, sigma2, 0);

	/* find the cross correlation peak */
	if (!findpeak(theccf, imsize, xpeak, ypeak, peakval)) 
		return 0;

	*xpeak = (*xpeak - imsize / 2) / imsize;
	*ypeak = (*ypeak - imsize / 2) / imsize; 
	return 1;
}

static void _clear(CmpackMatch *cfg)
{
	int imsize = cfg->fft_imsize;
	if (cfg->fft_theccf) {
		freeFloatArray(cfg->fft_theccf, imsize, imsize);
		cfg->fft_theccf = NULL;
	}
	if (cfg->fft_n[0]) {
		freeFloatArray(cfg->	fft_n[0], imsize, imsize);
		cfg->fft_n[0] = NULL;
	}
	if (cfg->fft_n[1]) {
		freeFloatArray(cfg->fft_n[1], imsize, imsize);
		cfg->fft_n[1] = NULL;
	}
	if (cfg->fft_fk[0]) {
		free_fft(cfg->fft_fk[0], imsize, imsize);
		cfg->fft_fk[0] = NULL;
	}
	if (cfg->fft_fk[1]) {
		free_fft(cfg->fft_fk[1], imsize, imsize);
		cfg->fft_fk[1] = NULL;
	}
	cfg->fft_imsize = 0;
}

static int _prepare(CmpackMatch *cfg, int imsize)
{
	if (imsize != cfg->fft_imsize) {
		_clear(cfg);
		allocFloatArray(&cfg->fft_theccf, imsize, imsize);
		allocFloatArray(&(cfg->fft_n[0]), imsize, imsize);
		allocFloatArray(&(cfg->fft_n[1]), imsize, imsize);
		alloc_fft(&(cfg->fft_fk[0]), imsize, imsize);
		alloc_fft(&(cfg->fft_fk[1]), imsize, imsize);
	}
	if (!cfg->fft_theccf || !cfg->fft_n[0] || !cfg->fft_n[1] || !cfg->fft_fk[0] || !cfg->fft_fk[1]) {
		_clear(cfg);
		return CMPACK_ERR_MEMORY;
	} else {
		cfg->fft_imsize = imsize;
		return 0;
	}
}

static double find_nearest(int i, CmpackMatch *cfg, double *m)
{
	if (cfg->i1[i] >= 0 && cfg->i1[i] != cfg->setref[0]) {
		/* Compute estimated destination coordinates */
		int j, j0 = -1;
		double xx, yy, rr = 1e99;
		if (cfg->i1[i] != cfg->setpos_ref_id) 
			Trafo(m,cfg->x1[i],cfg->y1[i],&xx,&yy);
		else
			Trafo(m,cfg->setpos_xy[0],cfg->setpos_xy[1],&xx,&yy);
		/* Select the nearest star */
		for (j=0; j<cfg->c2; j++) {
			if (cfg->i2[j] >= 0 && cfg->i2[j] != cfg->setref[1]) {		
				double dx = xx-cfg->x2[j];
				double dy = yy-cfg->y2[j];
				double dr = (dx*dx+dy*dy);
				if (dr <= 100 && dr < rr) { rr = dr; j0 = j; }
			}
		}
		if (j0 >= 0) 
			return rr;
	}
	return -1;
}

int PhiLnRMatch(CmpackMatch *cfg)
{
	int retval, i, j, i1, iphi, cat, obj, obj0, obj1, dok[2], mstar = 0, nstar = 0;
	double dphi, Phi[2], peakval[2], dx[2], dy[2], *d, m[9], tol2, mad;
	double **X[2], ***n;			/* x[cat][obj][coord] and (transformed) copy */ 
	double xpeak, ypeak, junk, phimax, amax, xrange;
	double xll[2][2], xur[2][2];	/* bounding boxes xll[cat][coord] */ 
	char msg[512];

	static const int imsize = 512;
	static const double phi1 = 0.0, phi2 = M_PI, dlnr = 1.0;
	static const double a1 = 0.1, a2 = 10.0; 

	match_frame_reset(cfg);

	printout(cfg->con,1,"Matching algorithm               : PhiLogR");
	
	/* Check parameters */
	if (cfg->nstar<=2) {
		printout(cfg->con, 0,"Number of identification stars muse be greater than 2");
		return CMPACK_ERR_INVALID_PAR;
	}
	if (cfg->nstar>=20) {
		printout(cfg->con, 0,"Number of identification stars muse be less than 20");
		return CMPACK_ERR_INVALID_PAR;
	}	  
	if (cfg->maxstar<cfg->nstar) {
		printout(cfg->con, 0,"Number of stars used muse be greater or equal to number of identification stars");
		return CMPACK_ERR_INVALID_PAR;
	}
	if (cfg->maxstar>=1000) {
		printout(cfg->con, 0,"Number of stars used for matching muse be less than 1000");
		return CMPACK_ERR_INVALID_PAR;
	}
	if (cfg->clip<=0) {
		printout(cfg->con, 0,"Clipping factor must be greater than zero");
		return CMPACK_ERR_INVALID_PAR;
	}

	/* Check data */
	if (cfg->c1<cfg->nstar) {
		printout(cfg->con, 0, "Too few stars in the reference file!");
		return CMPACK_ERR_FEW_POINTS_SRC;
	}
	if (cfg->c2<cfg->nstar) {
		printout(cfg->con, 0, "Too few stars in the source file!");
		return CMPACK_ERR_FEW_POINTS_SRC;
	}

	/* allocate space for transformed and shifted versions of cats */
	X[0] = allocDouble2DArray(2, cfg->c1);
	if (!X[0]) {
		return CMPACK_ERR_MEMORY;
	}
	X[1] = allocDouble2DArray(2, cfg->c2);
	if (!X[1]) {
		freeDouble2DArray(X[0], 2, cfg->c1);
		return CMPACK_ERR_MEMORY;
	}

	/* allocate image arrays, fft's */
	retval = _prepare(cfg, imsize);
	if (retval!=0)
		return CMPACK_ERR_MEMORY;
	
	/* make images of pairs */
	dphi = phi2 - phi1;
	n = cfg->fft_n;
	for (cat = 0; cat < 2; cat++) {
		int nobj = (cat==0 ? cfg->c1 : cfg->c2);
		const double *x = (cat==0 ? cfg->x1 : cfg->x2), *y = (cat==0 ? cfg->y1 : cfg->y2);
		for (obj0 = 0; obj0 < nobj; obj0++) {
			for (obj1 = 0; obj1 < nobj; obj1++) {
				double Dx = x[obj1] - x[obj0];
				double Dy = y[obj1] - y[obj0];
				double rr = Dx * Dx + Dy * Dy;
				if (rr > 0.0) {
					double lnr = 0.5 * log(rr);
					double phi = atan2(Dy, Dx) - phi1;
					int ix = (int)(imsize * (phi/dphi - floor(phi/dphi)));
					int iy = (int)(imsize * (lnr/dlnr - floor(lnr/dlnr)));
					n[cat][iy][ix] += 1.0;
				}
			}
		}
	} 

	/* do the crosscorrelation */
	retval = correlate(cfg, &xpeak, &ypeak, &junk);
	if (!retval) {
		printout(cfg->con,1,"Coincidences not found!");
		freeDouble2DArray(X[0], 2, cfg->c1);
		freeDouble2DArray(X[1], 2, cfg->c2);
		return CMPACK_ERR_MATCH_NOT_FOUND;
	}
	amax = exp(dlnr * ypeak);
	if (amax < a1 || amax > a2) {
		printout(cfg->con,1,"Coincidences not found!");
		freeDouble2DArray(X[0], 2, cfg->c1);
		freeDouble2DArray(X[1], 2, cfg->c2);
		return CMPACK_ERR_MATCH_NOT_FOUND;
	}

	/* for each possible orientation... */
	phimax = phi1 + dphi * xpeak; 
	Phi[0] = phimax;
	Phi[1] = phimax + M_PI;
	memset(dok, 0, 2*sizeof(int));
	for (iphi = 0; iphi < 2; iphi++) {
		/* rotate the 1st list of coords */
		double c = cos(Phi[iphi]);
		double s = sin(Phi[iphi]);
		for (obj = 0; obj < cfg->c1; obj++) {
			X[0][obj][0] = amax * (c * cfg->x1[obj] - s * cfg->y1[obj]);
			X[0][obj][1] = amax * (s * cfg->x1[obj] + c * cfg->y1[obj]);
		}
		/* and make copy of second */	
		for (obj = 0; obj < cfg->c2; obj++) {
			X[1][obj][0] = cfg->x2[obj];
			X[1][obj][1] = cfg->y2[obj];
		}
		/* for each cat... */
		for (cat = 0; cat < 2; cat++) {
			int nobj = (cat==0 ? cfg->c1 : cfg->c2);
			/* figure out the bounding boxes */
			for (i = 0; i < 2; i++) {
				xll[cat][i] = 1.e20;
				xur[cat][i] = -1.e20;
				for (obj = 0; obj < nobj; obj++) {
					if (X[cat][obj][i] < xll[cat][i]) {
						xll[cat][i] = X[cat][obj][i];
					}
					if (X[cat][obj][i] > xur[cat][i]) {
						xur[cat][i] = X[cat][obj][i];
					}
				}
			}
			/* and shift the origin to xll */
			for (obj = 0; obj < nobj; obj++) {
				for (i = 0; i < 2; i++) {
					X[cat][obj][i] -= xll[cat][i];
				}
			}
		}
		/* figure out the biggest x-range */
		xrange = 0;
		for (cat = 0; cat < 2; cat++) {
			for (i = 0; i < 2; i++) {
				if ((xur[cat][i] - xll[cat][i]) > xrange) {
					xrange = xur[cat][i] - xll[cat][i];
				}
			}
		}
	
		/* now generate the two images of x,y coords */
		for (cat = 0; cat < 2; cat++) {
			int ix, iy, nobj = (cat==0 ? cfg->c1 : cfg->c2);
			for (iy = 0; iy < imsize; iy++) {
				for (ix = 0; ix < imsize; ix++) {
					n[cat][iy][ix] = 0.0;
				}
			}
			for (obj = 0; obj < nobj; obj++) {
				int ix = (int)floor(imsize * X[cat][obj][0] / xrange);
				int iy = (int)floor(imsize * X[cat][obj][1] / xrange);
				if (ix >= 0 && ix < imsize && iy >= 0 && iy < imsize)
					n[cat][iy][ix] += 1.0;
			}
		}

		/* now we cross correlate and smooth */
		if (correlate(cfg, &xpeak, &ypeak, &(peakval[iphi]))) {
			/* compute shift */
			dx[iphi] = xrange * xpeak + xll[1][0] - xll[0][0]; 
			dy[iphi] = xrange * ypeak + xll[1][1] - xll[0][1];
			dok[iphi] = 1;
		}
	}	
	if (!dok[0] && !dok[1]) {
		printout(cfg->con,1,"Coincidences not found!");
		freeDouble2DArray(X[0], 2, cfg->c1);
		freeDouble2DArray(X[1], 2, cfg->c2);
		return CMPACK_ERR_MATCH_NOT_FOUND;
	}

	/* choose the highest peak */
	if (dok[0] && dok[1])
		iphi = (peakval[0] > peakval[1] ? 0 : 1); 
	else 
		iphi = (dok[0] ? 0 : 1);

	M(0,0) = amax*cos(Phi[iphi]);
    M(0,1) = -amax*sin(Phi[iphi]);
    M(0,2) = dx[iphi];
    M(1,0) = amax*sin(Phi[iphi]);
    M(1,1) = amax*cos(Phi[iphi]);
    M(1,2) = dy[iphi];
    M(2,0) = 0.0;
    M(2,1) = 0.0;
    M(2,2) = 1.0;

	/* Compute list of absolute deviations */
	d = (double*)cmpack_malloc(cfg->c1*sizeof(double));
	mstar = 0;
    for (i1=0; i1<cfg->c1; i1++) {
		double r2 = find_nearest(i1, cfg, m);
		if (r2>=0) 
			d[mstar++] = sqrt(r2);
	}

	if (mstar==0) {
		printout(cfg->con,1,"Coincidences not found!");
		freeDouble2DArray(X[0], 2, cfg->c1);
		freeDouble2DArray(X[1], 2, cfg->c2);
		return CMPACK_ERR_MATCH_NOT_FOUND;
	}

	mad = median(mstar, d);
	sprintf(msg,"Median absolute deviations       : %.4f", mad);
	printout(cfg->con,1, msg);

	tol2 = fmax(0.0001, 3.0*cfg->clip*cfg->clip*1.4826*1.4826*mad*mad+MACHEPS);

	sprintf(msg,"Tolerance                        : %.2f", sqrt(tol2));
	printout(cfg->con,1, msg);
	sprintf(msg,"Transformation matrix            : ");
	printout(cfg->con,1, msg);
	sprintf(msg,"   %15.3f %15.3f %15.3f",M(0,0),M(0,1),M(0,2));
	printout(cfg->con,1, msg);
	sprintf(msg,"   %15.3f %15.3f %15.3f",M(1,0),M(1,1),M(1,2));
	printout(cfg->con,1, msg);
	sprintf(msg,"   %15.3f %15.3f %15.3f",M(2,0),M(2,1),M(2,2));
	printout(cfg->con,1, msg);

	/* Set cross-references for all stars on input frame */
	nstar = 0;
	if (cfg->setref[0]>0 && cfg->setref[1]>0) {
		/* Client defined match for a minor body object */
		for (j=0; j<cfg->c2; j++) {
			if (cfg->i2[j] == cfg->setref[1]) {
				cfg->xref[j] = cfg->setref[0];
				nstar++;
				break;
			}
		}
	}
    for (i=0; i<cfg->c1; i++) {
		if (cfg->i1[i] >= 0 && cfg->i1[i] != cfg->setref[0]) {		
			/* Compute estimated destination coordinates */
			double xx, yy, rr = 1e99;
			int j, j0 = -1;
			if (cfg->i1[i] != cfg->setpos_ref_id) 
				Trafo(m,cfg->x1[i],cfg->y1[i],&xx,&yy);
			else
				Trafo(m,cfg->setpos_xy[0],cfg->setpos_xy[1],&xx,&yy);
			/* Select the nearest star */
			for (j=0; j<cfg->c2; j++) {
				if (cfg->i2[j] >= 0 && cfg->xref[j] < 0) {		
					double dx = xx-cfg->x2[j];
					double dy = yy-cfg->y2[j];
					double dr = (dx*dx+dy*dy);
					if (dr<rr) { rr = dr; j0 = j; }
				}
			}
			if ((j0>=0)&&(rr<tol2)) {
				cfg->xref[j0] = cfg->i1[i];
				nstar++;
			}
		}
    }

	cfg->matched_stars = nstar;
	cmpack_matrix_init(&cfg->matrix, M(0,0), M(1,0), M(0,1), M(1,1), M(0,2), M(1,2));

	freeDouble2DArray(X[0], 2, cfg->c1);
	freeDouble2DArray(X[1], 2, cfg->c2);

	return 0;
}

void PhiLnRClear(CmpackMatch *cfg)
{
	_clear(cfg);
}
