/* find position of maximum, subpixel estimation
 *
 * Copyright: 2008, Nottingham Trent University
 * Author: Tom Vajzovic
 * Written on: 2008-02-07
 *
 * 25/1/11
 * 	- gtk-doc
 */

/*

	This file is part of VIPS.

	VIPS is free software; you can redistribute it and/or modify
	it under the terms of the GNU Lesser 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 Lesser General Public License for more details.

	You should have received a copy of the GNU Lesser General Public License
	along with this program; if not, write to the Free Software
	Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA
	02110-1301  USA

 */

/*

	These files are distributed with VIPS - http://www.vips.ecs.soton.ac.uk

 */

#ifdef HAVE_CONFIG_H
#include <config.h>
#endif /*HAVE_CONFIG_H */
#include <glib/gi18n-lib.h>

#include <stdlib.h>
#include <vips/vips.h>
#include <vips/vips7compat.h>

#define MOST_OF(A, B) ((A) > 0.9 * (B))
#define LITTLE_OF(A, B) ((B) < 0.1 * (B))

/**
 * im_maxpos_subpel:
 * @in: input image
 * @x: output position of maximum
 * @y: output position of maximum
 *
 * This function implements:
 *
 *   "Extension of Phase Correlation to Subpixel Registration"
 *   by H. Foroosh, from IEEE trans. Im. Proc. 11(3), 2002.
 *
 * If the best three matches in the correlation are aranged:
 *
 *   02   or   01
 *   1         2
 *
 * then we return a subpixel match using the ratio of correlations in the
 * vertical and horizontal dimension.
 *
 * (xs[0], ys[0]) is the best integer alignment
 * (xs[ use_x ], ys[ use_x ]) is equal in y and (+/-)1 off in x
 * (xs[ use_y ], ys[ use_y ]) is equal in x and (+/-)1 off in y
 *
 * Alternatively if the best four matches in the correlation are aranged in
 * a square:
 *
 *   01  or  03  or  02  or  03
 *   32      12      31      21
 *
 * then we return a subpixel match weighting with the sum the two on each
 * side over the sum of all four, but only if all four of them are very
 * close to the best, and the fifth is nowhere near.
 *
 * This alternative method is not described by Foroosh, but is often the
 * case where the match is close to n-and-a-half pixels in both dimensions.
 *
 * See also: im_maxpos(), im_min(), im_stats().
 *
 * Returns: 0 on success, -1 on error
 */

int
im_maxpos_subpel(IMAGE *in, double *x, double *y)
{
#define FUNCTION_NAME "im_maxpos_subpel"

	int xs[5];
	int ys[5];
	double vals[5];
	int xa, ya, xb, yb;
	double vxa, vya, vxb, vyb;

	if (im_maxpos_vec(in, xs, ys, vals, 5))
		return -1;

#define WRAP_TEST_RETURN() \
\
	/* wrap around if we have alignment -1 < d <= 0 */ \
	/* (change it to: size - 1 <= d < size ) */ \
\
	if (!xa && in->Xsize - 1 == xb) \
		xa = in->Xsize; \
\
	else if (!xb && in->Xsize - 1 == xa) \
		xb = in->Xsize; \
\
	if (!ya && in->Ysize - 1 == yb) \
		ya = in->Ysize; \
\
	else if (!yb && in->Ysize - 1 == ya) \
		yb = in->Ysize; \
\
	if (1 == abs(xb - xa) && 1 == abs(yb - ya)) { \
		*x = ((double) xa) + ((double) (xb - xa)) * (vxb / (vxa + vxb)); \
		*y = ((double) ya) + ((double) (yb - ya)) * (vyb / (vya + vyb)); \
		return 0; \
	}

#define TEST3(A, B) \
	if (xs[0] == xs[A] && ys[0] == ys[B]) { \
		xa = xs[0]; \
		ya = ys[0]; \
		xb = xs[B]; \
		yb = ys[A]; \
		vxa = vals[0]; \
		vya = vals[0]; \
		vxb = vals[B]; \
		vyb = vals[A]; \
		WRAP_TEST_RETURN() \
	}

	TEST3(1, 2)
	TEST3(2, 1)

	if (MOST_OF(vals[1], vals[0]) && MOST_OF(vals[2], vals[0]) &&
		MOST_OF(vals[3], vals[0]) && LITTLE_OF(vals[4], vals[0])) {

#define TEST4(A, B, C, D, E, F, G, H) \
	if (xs[A] == xs[B] && xs[C] == xs[D] && ys[E] == ys[F] && ys[G] == ys[H]) { \
		xa = xs[A]; \
		xb = xs[C]; \
		ya = ys[E]; \
		yb = ys[G]; \
		vxa = vals[A] + vals[B]; \
		vxb = vals[C] + vals[D]; \
		vya = vals[E] + vals[F]; \
		vyb = vals[G] + vals[H]; \
		WRAP_TEST_RETURN() \
	}

		TEST4(0, 3, 1, 2, 0, 1, 2, 3)
		TEST4(0, 1, 2, 3, 0, 3, 1, 2)
		TEST4(0, 3, 1, 2, 0, 2, 1, 3)
		TEST4(0, 2, 1, 3, 0, 3, 1, 2)
	}

	im_warn(FUNCTION_NAME, "registration performed to nearest pixel only: correlation does not have the expected distribution for sub-pixel registration");
	*x = (double) xs[0];
	*y = (double) ys[0];
	return 0;
}
