/*
* Copyright (C) 2007-2008 Anael Orlinski
*
* This file is part of Panomatic.
*
* Panomatic 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.
* 
* Panomatic 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 Panomatic; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include <iostream>
#include <float.h>
#include <math.h>
//#include "Tracer.h"

#include "Homography.h"
#include <boost/foreach.hpp>

using namespace std;

namespace lfeat
{

const int Homography::kNCols = 8;

inline int fsign(double x)
{
	return (x > 0 ? 1 : (x < 0) ? -1 : 0);
}

bool Givens(double **C, double *d, double *x, double *r, int N, int n, int want_r);

Homography::Homography(void) : _nMatches(0)
{
}

void Homography::allocMemory(int iNMatches)
{
	int aNRows = iNMatches * 2;
	_Amat = new double*[aNRows];
	for(int aRowIter = 0; aRowIter < aNRows; ++aRowIter)
		_Amat[aRowIter] = new double[kNCols];

	_Bvec = new double[aNRows];
	_Rvec = new double[aNRows];
	_Xvec = new double[kNCols];

	_nMatches = iNMatches;
}

void Homography::freeMemory()
{
	// free memory
	delete[] _Bvec;
	delete[] _Rvec;
	delete[] _Xvec;
	for(int aRowIter = 0; aRowIter < _nMatches; ++aRowIter)
		delete[] _Amat[aRowIter];
	delete[] _Amat;

	// reset number of matches
	_nMatches = 0;

}

Homography::~Homography()
{
	if (_nMatches)
		freeMemory();
}

void Homography::initMatchesNormalization(PointMatchVector_t& iMatches)
{
	// for each set of points (img1, img2), find the vector
	// to apply to all points to have coordinates centered
	// on the barycenter.

	_v1x = 0;
	_v2x = 0;
	_v1y = 0;
	_v2y = 0;

	//estimate the center of gravity
	BOOST_FOREACH(PointMatchPtr& aMatchIt, iMatches)
	{
		//aMatchIt->print();

		_v1x += aMatchIt->_img1_x;
		_v1y += aMatchIt->_img1_y;
		_v2x += aMatchIt->_img2_x;
		_v2y += aMatchIt->_img2_y;
	}

	_v1x /= (double)iMatches.size();
	_v1y /= (double)iMatches.size();
	_v2x /= (double)iMatches.size();
	_v2y /= (double)iMatches.size();
}


ostream& operator<< (ostream& o, const Homography& H)
{
	o << H._H[0][0] << "\t" << H._H[0][1] << "\t" << H._H[0][2] << endl;
	o << H._H[1][0] << "\t" << H._H[1][1] << "\t" << H._H[1][2] << endl;
	o << H._H[2][0] << "\t" << H._H[2][1] << "\t" << H._H[2][2] << endl;

	return o;
}

void Homography::addMatch(int iIndex, PointMatch& iMatch)
{
	int aRow = iIndex * 2;
	double aI1x = iMatch._img1_x - _v1x;
	double aI1y = iMatch._img1_y - _v1y;
	double aI2x = iMatch._img2_x - _v2x;
	double aI2y = iMatch._img2_y - _v2y;

	_Amat[aRow][0] = 0;
	_Amat[aRow][1] = 0;
	_Amat[aRow][2] = 0;
	_Amat[aRow][3] = - aI1x;
	_Amat[aRow][4] = - aI1y;
	_Amat[aRow][5] = -1;
	_Amat[aRow][6] = aI1x * aI2y;
	_Amat[aRow][7] = aI1y * aI2y;

	_Bvec[aRow] = aI2y;

	aRow++;

	_Amat[aRow][0] = aI1x;
	_Amat[aRow][1] = aI1y;
	_Amat[aRow][2] = 1;
	_Amat[aRow][3] = 0;
	_Amat[aRow][4] = 0;
	_Amat[aRow][5] = 0;
	_Amat[aRow][6] = - aI1x * aI2x;
	_Amat[aRow][7] = - aI1y * aI2x;

	_Bvec[aRow] = - aI2x;
}

void Homography::transformPoint(double iX, double iY, double& oX, double& oY)
{
	double aX = iX - _v1x;
	double aY = iY - _v1y;
	double aK = double(1. / (_H[2][0] * aX + _H[2][1] * aY + _H[2][2]));

	oX = aK * (_H[0][0] * aX + _H[0][1] * aY + _H[0][2]) + _v2x;
	oY = aK * (_H[1][0] * aX + _H[1][1] * aY + _H[1][2]) + _v2y;
}

bool Homography::estimate(PointMatchVector_t& iMatches)
{
	// check the number of matches we need at least 4.
	if (iMatches.size() < 4)
	{
		//TRACE_ERROR("At least 4 matches are needed for homography");
		return false;
	}
	
	// create matrices for least-squares solving
	
	// if there is a different size of matrices set, delete them
	if (_nMatches != (int)iMatches.size() && _nMatches != 0)
		freeMemory();

	// if there is no memory allocated, alloc memory
	if (_nMatches == 0)
		allocMemory((int)iMatches.size());

	// fill the matrices and vectors with points
	int aFillRow = 0;
	BOOST_FOREACH(PointMatchPtr aPM, iMatches)
	{
		addMatch(aFillRow, *aPM);
		aFillRow++;
	}

	// solve the system
	if (!Givens(_Amat, _Bvec, _Xvec, _Rvec, _nMatches*2, kNCols, 0))
	{
		//TRACE_ERROR("Failed to solve the linear system");
		return false;
	}

	// fill the homography matrix with the values.

	_H[0][0] = _Xvec[0];
	_H[0][1] = _Xvec[1];
	_H[0][2] = _Xvec[2];

	_H[1][0] = _Xvec[3];
	_H[1][1] = _Xvec[4];
	_H[1][2] = _Xvec[5];

	_H[2][0] = _Xvec[6];
	_H[2][1] = _Xvec[7];
	_H[2][2] = 1.0;

	return true;
	
}




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

Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
(QR-decomposition). Direct implementation of the algorithm
presented in H.R.Schwarz: Numerische Mathematik, 'equation'
number (7.33)

If 'd == NULL', d is not accesed: the routine just computes the QR
decomposition of C and exits.

If 'want_r == 0', r is not rotated back (\hat{r} is returned
instead).

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

bool Givens(double **C, double *d, double *x, double *r, int N, int n, int want_r)
{
	int i, j, k;
	double w, gamma, sigma, rho, temp;
	double epsilon = DBL_EPSILON;	/* FIXME (?) */

	/*
	* First, construct QR decomposition of C, by 'rotating away'
	* all elements of C below the diagonal. The rotations are
	* stored in place as Givens coefficients rho.
	* Vector d is also rotated in this same turn, if it exists
	*/
	for (j = 0; j < n; j++) {
		for (i = j + 1; i < N; i++) {
			if (C[i][j]) {
				if (fabs(C[j][j]) < epsilon * fabs(C[i][j])) {
					/* find the rotation parameters */
					w = -C[i][j];
					gamma = 0;
					sigma = 1;
					rho = 1;
				} else {
					w = fsign(C[j][j]) * sqrt(C[j][j] * C[j][j] + C[i][j] * C[i][j]);
					if (w == 0)
						return false;
					gamma = C[j][j] / w;
					sigma = -C[i][j] / w;
					rho = (fabs(sigma) < gamma) ? sigma : fsign(sigma) / gamma;
				}
				C[j][j] = w;
				C[i][j] = rho;	/* store rho in place, for later use */
				for (k = j + 1; k < n; k++) {
					/* rotation on index pair (i,j) */
					temp = gamma * C[j][k] - sigma * C[i][k];
					C[i][k] = sigma * C[j][k] + gamma * C[i][k];
					C[j][k] = temp;

				}
				if (d) {	/* if no d vector given, don't use it */
					temp = gamma * d[j] - sigma * d[i];		/* rotate d */
					d[i] = sigma * d[j] + gamma * d[i];
					d[j] = temp;
				}
			}
		}
	}

	if (!d)			/* stop here if no d was specified */
		return true;

	/* solve R*x+d = 0, by backsubstitution */
	for (i = n - 1; i >= 0; i--) {
		double s = d[i];

		r[i] = 0;		/* ... and also set r[i] = 0 for i<n */
		for (k = i + 1; k < n; k++)
			s += C[i][k] * x[k];
		if (C[i][i] == 0)
			return false;
		x[i] = -s / C[i][i];
	}

	for (i = n; i < N; i++)
		r[i] = d[i];		/* set the other r[i] to d[i] */

	if (!want_r)		/* if r isn't needed, stop here */
		return true;

	/* rotate back the r vector */
	for (j = n - 1; j >= 0; j--) {
		for (i = N - 1; i >= 0; i--) {
			if ((rho = C[i][j]) == 1) {		/* reconstruct gamma, sigma from stored rho */
				gamma = 0;
				sigma = 1;
			} else if (fabs(rho) < 1) {
				sigma = rho;
				gamma = sqrt(1 - sigma * sigma);
			} else {
				gamma = 1 / fabs(rho);
				sigma = fsign(rho) * sqrt(1 - gamma * gamma);
			}
			temp = gamma * r[j] + sigma * r[i];		/* rotate back indices (i,j) */
			r[i] = -sigma * r[j] + gamma * r[i];
			r[j] = temp;
		}
	}
	return true;
}

}