/*
  This file is part of the Astrometry.net suite.
  Copyright 2009, 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-builder.h"
#include "quad-utils.h"
#include "mathutil.h"
#include "errors.h"
#include "log.h"

struct quad {
	unsigned int star[DQMAX];
};
typedef struct quad quad;

static void check_scale(quadbuilder_t* qb, pquad_t* pq) {
	double *sA, *sB;
	double s2;
	double Bx=0, By=0;
	double invscale;
	double ABx, ABy;
	Unused anbool ok;
	if (!(qb->check_scale_low || qb->check_scale_high))
		return;
	sA = qb->starxyz + pq->iA * 3;
	sB = qb->starxyz + pq->iB * 3;
	// s2: squared AB dist
	s2 = distsq(sA, sB, 3);
	pq->scale_ok = TRUE;
	if (qb->check_scale_low && s2 < qb->quadd2_low)
		pq->scale_ok = FALSE;
	if (pq->scale_ok && qb->check_scale_high && s2 > qb->quadd2_high)
		pq->scale_ok = FALSE;
	if (!pq->scale_ok) {
		qb->nbadscale++;
		return;
	}
	star_midpoint(pq->midAB, sA, sB);
	pq->scale_ok = TRUE;
	pq->staridA = qb->starinds[pq->iA];
	pq->staridB = qb->starinds[pq->iB];
	ok = star_coords(sA, pq->midAB, TRUE, &pq->Ay, &pq->Ax);
	assert(ok);
	ok = star_coords(sB, pq->midAB, TRUE, &By, &Bx);
	assert(ok);
	ABx = Bx - pq->Ax;
	ABy = By - pq->Ay;
	invscale = 1.0 / (ABx*ABx + ABy*ABy);
	pq->costheta = (ABy + ABx) * invscale;
	pq->sintheta = (ABy - ABx) * invscale;
	//nabok++;
}

static int
check_inbox(pquad_t* pq, int* inds, int ninds, double* stars) {
	int i, ind;
	double* starpos;
	double Dx=0, Dy=0;
	double ADx, ADy;
	double x, y;
	int destind = 0;
	anbool ok;
	for (i=0; i<ninds; i++) {
		double r;
		ind = inds[i];
		starpos = stars + ind*3;
		ok = star_coords(starpos, pq->midAB, TRUE, &Dy, &Dx);
		if (!ok)
			continue;
		ADx = Dx - pq->Ax;
		ADy = Dy - pq->Ay;
		x =  ADx * pq->costheta + ADy * pq->sintheta;
		y = -ADx * pq->sintheta + ADy * pq->costheta;
		// make sure it's in the circle centered at (0.5, 0.5)...
		// (x-1/2)^2 + (y-1/2)^2   <=   r^2
		// x^2-x+1/4 + y^2-y+1/4   <=   (1/sqrt(2))^2
		// x^2-x + y^2-y + 1/2     <=   1/2
		// x^2-x + y^2-y           <=   0
		r = (x*x - x) + (y*y - y);
		if (r > 0.0)
			continue;
		inds[destind] = ind;
		destind++;
	}
	return destind;
}

/**
 inbox, ninbox: the stars we have to work with.
 starinds: the star identifiers (indexed by the contents of 'inbox')
 - ie, starinds[inbox[0]] is an externally-recognized star identifier.
 q: where we record the star identifiers
 starnum: which star we're adding: eg, A=0, B=1, C=2, ... dimquads-1.
 beginning: the first index in "inbox" to assign to star 'starnum'.
 */
static void add_interior_stars(quadbuilder_t* qb,
							   int ninbox, int* inbox, quad* q,
							   int starnum, int dimquads, int beginning) {
	int i;
	for (i=beginning; i<ninbox; i++) {
		int iC = inbox[i];
		q->star[starnum] = qb->starinds[iC];
		// Did we just add the last star?
		if (starnum == dimquads-1) {
			if (qb->check_full_quad &&
				!qb->check_full_quad(qb, q->star, dimquads, qb->check_full_quad_token))
				continue;

			qb->add_quad(qb, q->star, qb->add_quad_token);
		} else {
			if (qb->check_partial_quad &&
				!qb->check_partial_quad(qb, q->star, starnum+1, qb->check_partial_quad_token))
				continue;
			// Recurse.
			add_interior_stars(qb, ninbox, inbox, q, starnum+1, dimquads, i+1);
		}
		if (qb->stop_creating)
			return;
	}
}

quadbuilder_t* quadbuilder_init() {
	quadbuilder_t* qb = calloc(1, sizeof(quadbuilder_t));
	return qb;
}

void quadbuilder_free(quadbuilder_t* qb) {
	free(qb->inbox);
	free(qb->pquads);
	free(qb);
}

int quadbuilder_create(quadbuilder_t* qb) {
	int iA=0, iB, iC, iD, newpoint;
	int ninbox;
	int i, j;
	int iAalloc;
	quad q;
	pquad_t* qb_pquads;

	// ensure the arrays are large enough...
	if (qb->Nstars > qb->Ncq) {
		// (free and malloc rather than realloc because we don't care about
		//  the previous contents)
		free(qb->inbox);
		free(qb->pquads);
		qb->Ncq = qb->Nstars;
		qb->inbox =  calloc(qb->Nstars, sizeof(int));
		qb->pquads = calloc(qb->Nstars * qb->Nstars, sizeof(pquad_t));
		if (!qb->inbox || !qb->pquads) {
			ERROR("quad-builder: failed to malloc qb->inbox or qb->pquads.  Nstars=%i.\n", qb->Nstars);
			return -1;
		}
	}

	qb_pquads = qb->pquads;

	/*
	  Each time through the "for" loop below, we consider a new
	  star ("newpoint").  First, we try building all quads that
	  have the new star on the diagonal (star B).  Then, we try
	  building all quads that have the star not on the diagonal
	  (star D).

	  Note that we keep the invariants iA < iB and iC < iD.
	*/
	memset(&q, 0, sizeof(quad));
	for (newpoint=0; newpoint<qb->Nstars; newpoint++) {
		pquad_t* pq;
		// quads with the new star on the diagonal:
		iB = newpoint;
		for (iA = 0; iA < newpoint; iA++) {
			pq = qb_pquads + iA*qb->Nstars + iB;
			pq->inbox = NULL;
			pq->ninbox = 0;
			pq->iA = iA;
			pq->iB = iB;

			check_scale(qb, pq);
			if (!pq->scale_ok)
				continue;

			q.star[0] = pq->staridA;
			q.star[1] = pq->staridB;

			pq->check_ok = TRUE;
			if (qb->check_AB_stars)
				pq->check_ok = qb->check_AB_stars(qb, pq, qb->check_AB_stars_token);
			if (!pq->check_ok)
				continue;

			// list the possible internal stars...
			ninbox = 0;
			for (iC = 0; iC < newpoint; iC++) {
				if ((iC == iA) || (iC == iB))
					continue;
				qb->inbox[ninbox] = iC;
				ninbox++;
			}
			// check which ones are inside the box...
			ninbox = check_inbox(pq, qb->inbox, ninbox, qb->starxyz);
			//if (!ninbox)
			//continue;
			if (ninbox && qb->check_internal_stars)
				ninbox = qb->check_internal_stars(qb, q.star[0], q.star[1], qb->inbox, ninbox, qb->check_internal_stars_token);
			//if (!ninbox)
			//continue;

			add_interior_stars(qb, ninbox, qb->inbox, &q, 2, qb->dimquads, 0);
			if (qb->stop_creating)
				goto theend;

			pq->inbox = malloc(qb->Nstars * sizeof(int));
			if (!pq->inbox) {
				ERROR("hpquads: failed to malloc pq->inbox.\n");
				exit(-1);
			}
			pq->ninbox = ninbox;
			memcpy(pq->inbox, qb->inbox, ninbox * sizeof(int));
			debug("iA=%i, iB=%i: saved %i 'inbox' entries.\n", iA, iB, ninbox);
		}
		iAalloc = iA;

		// quads with the new star not on the diagonal:
		iD = newpoint;
		for (iA = 0; iA < newpoint; iA++) {
			for (iB = iA + 1; iB < newpoint; iB++) {
				pq = qb_pquads + iA*qb->Nstars + iB;
				if (!(pq->scale_ok && pq->check_ok))
					continue;
				// check if this new star is in the box.
				qb->inbox[0] = iD;
				ninbox = check_inbox(pq, qb->inbox, 1, qb->starxyz);
				if (!ninbox)
					continue;
				if (qb->check_internal_stars)
					ninbox = qb->check_internal_stars(qb, q.star[0], q.star[1], qb->inbox, ninbox, qb->check_internal_stars_token);
				if (!ninbox)
					continue;

				pq->inbox[pq->ninbox] = iD;
				pq->ninbox++;

				q.star[0] = pq->staridA;
				q.star[1] = pq->staridB;

				add_interior_stars(qb, pq->ninbox, pq->inbox, &q, 2, qb->dimquads, 0);
				if (qb->stop_creating) {
					iA = iAalloc;
					goto theend;
				}
			}
		}
	}
 theend:
	for (i=0; i<imin(qb->Nstars, newpoint+1); i++) {
		int lim = (i == newpoint) ? iA : i;
		for (j=0; j<lim; j++) {
			pquad_t* pq = qb_pquads + j*qb->Nstars + i;
			free(pq->inbox);
			pq->inbox = NULL;
		}
	}
	return 0;
}

