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

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.

**************************************************************/
#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>

#include "comfun.h"
#include "console.h"
#include "cholesky.h"
#include "robmean.h"
#include "cmpack_common.h"
#include "cmpack_match.h"
#include "matfun.h"
#include "matsolve.h"
#include "matstack.h"
#include "match.h"

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

static int _trafo(CmpackWcs *wcs1, CmpackWcs *wcs2, double x1, double y1, double *x2, double *y2)
{
	double lng, lat;
	return cmpack_wcs_p2w(wcs1, x1, y1, &lng, &lat)==0 && cmpack_wcs_w2p(wcs2, lng, lat, x2, y2);
}

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

int WcsMatch(CmpackMatch *cfg, CmpackMatchFrame *lc)
{
	int		i, j, j0, mstar, nstar = 0;
	double	xx,yy,rr,dx,dy,dr,mad,tol2,*d;
	char 	msg[256];

	match_frame_reset(lc);

	printout(cfg->con,1,"Matching algorithm               : WCS-based");

	/* Check parameters */
	if (cfg->clip<=0) {
		printout(cfg->con, 0,"Clipping factor must be greater than zero");
		return CMPACK_ERR_INVALID_PAR;
	}

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

	/* Compute list of absolute deviations */
	d = cmpack_malloc((lc->c1)*sizeof(double));
	mstar = 0;
    for (i=0; i<lc->c1; i++) {
	    /* Compute estimated destination coordinates */
        _trafo(lc->wcs1,lc->wcs2,lc->x1[i],lc->y1[i],&xx,&yy);
        /* Select the nearest star */
        rr = MACHMAX; j0 = -1;
        for (j=0; j<lc->c2; j++) {
	        if (lc->i2[j]<0) {
		        dx = xx-lc->x2[j];
		        dy = yy-lc->y2[j];
		        dr = (dx*dx+dy*dy);
		        if (dr<rr) { rr = dr; j0 = j; }
	        }
        }
        if (j0>=0) 
			d[mstar++] = rr;
	}
	if (mstar>0) {
		mad = median(mstar, d);
		
		sprintf(msg,"Median absolute deviations       : %.4f", mad);
		printout(cfg->con,1, msg);

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

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

		/* Set cross-references for all stars on input frame */
		nstar = 0;
		for (i=0; i<lc->c1; i++) {
			/* Compute estimated destination coordinates */
			_trafo(lc->wcs1,lc->wcs2,lc->x1[i],lc->y1[i],&xx,&yy);
			/* Select the nearest star */
			rr = MACHMAX; j0 = -1;
			for (j=0; j<lc->c2; j++) {
				if (lc->i2[j]<0) {
					dx = xx-lc->x2[j];
					dy = yy-lc->y2[j];
					dr = (dx*dx+dy*dy);
					if (dr<rr) { rr = dr; j0 = j; }
				}
			}
			if ((j0>=0)&&(rr<tol2)) {
				lc->i2[j0] = i+1;
				nstar++;
			}
		}

		cmpack_free(d);
	}

	if (nstar==0) {
		printout(cfg->con,1,"Coincidences not found!");
		return CMPACK_ERR_MATCH_NOT_FOUND;
	}
	lc->mstar = nstar;
	return 0;
}
