/*
  This file is part of the Astrometry.net suite.
  Copyright 2006-2009 Dustin Lang, Keir Mierle and Sam Roweis.
  Copyright 2012 Dustin Lang.

  The Astrometry.net suite 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, version 2.

  The Astrometry.net suite 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 the Astrometry.net suite ; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
*/

#include <assert.h>

#include "quad-utils.h"
#include "starutil.h"
#include "codefile.h"
#include "starkd.h"
#include "errors.h"
#include "log.h"

void quad_compute_star_code(const double* starxyz, double* code, int dimquads) {
	double Ax=0, Ay=0;
	double Bx=0, By=0;
	double ABx, ABy;
	double scale, invscale;
	double costheta, sintheta;
	double midAB[3];
	Unused anbool ok;
	int i;
	const double *sA, *sB;

	sA = starxyz;
	sB = starxyz + 3;
	star_midpoint(midAB, sA, sB);
	ok = star_coords(sA, midAB, TRUE, &Ay, &Ax);
	assert(ok);
	ok = star_coords(sB, midAB, TRUE, &By, &Bx);
	assert(ok);
	ABx = Bx - Ax;
	ABy = By - Ay;
	scale = (ABx * ABx) + (ABy * ABy);
	invscale = 1.0 / scale;
	costheta = (ABy + ABx) * invscale;
	sintheta = (ABy - ABx) * invscale;

	for (i=2; i<dimquads; i++) {
        const double* starpos;
        double Dx=0, Dy=0;
        double ADx, ADy;
        double x, y;
		starpos = starxyz + 3*i;
		ok = star_coords(starpos, midAB, TRUE, &Dy, &Dx);
		assert(ok);
		ADx = Dx - Ax;
		ADy = Dy - Ay;
		x =  ADx * costheta + ADy * sintheta;
		y = -ADx * sintheta + ADy * costheta;
		code[2*(i-2)+0] = x;
		code[2*(i-2)+1] = y;
	}
}

void quad_flip_parity(const double* code, double* flipcode, int dimcode) {
	int i;
	// swap CX <-> CY, DX <-> DY.
	for (i=0; i<dimcode/2; i++) {
		// use tmp in code "code" == "flipcode"
		double tmp;
		tmp = code[2*i+1];
		flipcode[2*i+1] = code[2*i+0];
		flipcode[2*i+0] = tmp;
	}
}

int quad_compute_code(const unsigned int* quad, int dimquads, startree_t* starkd, 
					  double* code) {
    int i;
	double starxyz[3 * DQMAX];
	for (i=0; i<dimquads; i++) {
		if (startree_get(starkd, quad[i], starxyz + 3*i)) {
			ERROR("Failed to get stars belonging to a quad.\n");
			return -1;
		}
	}
    quad_compute_star_code(starxyz, code, dimquads);
	return 0;
}

anbool quad_obeys_invariants(unsigned int* quad, double* code,
						   int dimquads, int dimcodes) {
	double sum;
	int i;
	// check the invariant that (cx + dx + ...) / (dimquads-2) <= 1/2
	sum = 0.0;
	for (i=0; i<(dimquads-2); i++)
		sum += code[2*i];
	sum /= (dimquads-2);
	if (sum > 0.5)
		return FALSE;

	// check the invariant that cx <= dx <= ....
	for (i=0; i<(dimquads-3); i++)
		if (code[2*i] > code[2*(i+1)])
			return FALSE;
	return TRUE;
}

void quad_enforce_invariants(unsigned int* quad, double* code,
							 int dimquads, int dimcodes) {
	double sum;
	int i;

	// here we add the invariant that (cx + dx + ...) / (dimquads-2) <= 1/2
	sum = 0.0;
	for (i=0; i<dimcodes/2; i++)
		sum += code[2*i];
	sum /= (dimcodes/2);
	if (sum > 0.5) {
		logdebug("Flipping code to ensure mean(x)<=0.5\n");
		// swap the labels of A,B.
		int tmp = quad[0];
		quad[0] = quad[1];
		quad[1] = tmp;
		// rotate the code 180 degrees.
		for (i=0; i<dimcodes; i++)
			code[i] = 1.0 - code[i];
	}

	// here we add the invariant that cx <= dx <= ....
	for (i=0; i<(dimquads-2); i++) {
		int j;
		int jsmallest;
		double smallest;
		double x1;
		double dtmp;
		int tmp;

		x1 = code[2*i];
		jsmallest = -1;
		smallest = x1;
		for (j=i+1; j<(dimquads-2); j++) {
			double x2 = code[2*j];
			if (x2 < smallest) {
				smallest = x2;
				jsmallest = j;
			}
		}
		if (jsmallest == -1)
			continue;
		j = jsmallest;
		// swap the labels.
		tmp = quad[i+2];
		quad[i+2] = quad[j+2];
		quad[j+2] = tmp;
		// swap the code values.
		dtmp = code[2*i];
		code[2*i] = code[2*j];
		code[2*j] = dtmp;
		dtmp = code[2*i+1];
		code[2*i+1] = code[2*j+1];
		code[2*j+1] = dtmp;
	}
}

