/****************************************************************************
* VCGLib                                                            o o     *
* Visual and Computer Graphics Library                            o     o   *
*                                                                _   O  _   *
* Copyright(C) 2004-2016                                           \/)\/    *
* Visual Computing Lab                                            /\/|      *
* ISTI - Italian National Research Council                           |      *
*                                                                    \      *
* All rights reserved.                                                      *
*                                                                           *
* 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 (http://www.gnu.org/licenses/gpl.txt)          *
* for more details.                                                         *
*                                                                           *
****************************************************************************/

#ifndef _VCG_MATH_POINTMATCHING_H
#define _VCG_MATH_POINTMATCHING_H

#include <vcg/math/quaternion.h>
#include <vcg/math/matrix44.h>

#include <Eigen/Dense>
#include <Eigen/Eigenvalues>
#include  <iostream>

namespace vcg
{

/*! \brief Compute cross covariance

It computes the cross covariance matrix of two set of 3D points P and X;
it returns also the barycenters of P and X.
Ref:

Besl, McKay
A method for registration of 3d Shapes
IEEE TPAMI Vol 14, No 2 1992

*/
template <class S >
void ComputeCrossCovarianceMatrix(const std::vector<Point3<S> > &spVec, Point3<S> &spBarycenter,
                                  const std::vector<Point3<S> > &tpVec, Point3<S> &tpBarycenter,
                                  Eigen::Matrix3d &m)
{
	assert(spVec.size() == tpVec.size());
	m.setZero();
	spBarycenter.SetZero();
	tpBarycenter.SetZero();
	Eigen::Vector3d spe;
	Eigen::Vector3d tpe;
	typename std::vector <Point3<S> >::const_iterator si, ti;
	for (si = spVec.begin(), ti = tpVec.begin(); si != spVec.end(); ++si, ++ti){
		spBarycenter += *si;
		tpBarycenter += *ti;
		si->ToEigenVector(spe);
		ti->ToEigenVector(tpe);
		m += spe*tpe.transpose();
	}
	spBarycenter /= double(spVec.size());
	tpBarycenter /= double(tpVec.size());
	spBarycenter.ToEigenVector(spe);
	tpBarycenter.ToEigenVector(tpe);
	m /= double(spVec.size());
	m -= spe*tpe.transpose();
}

/*! \brief Compute the roto-translation that applied to PMov bring them onto Pfix
 * Rotation is computed as a quaternion.
 *
 * E.g. it find a matrix such that:
 *
 *       Pfix[i] = res * Pmov[i]
 *
 * Ref:
 * Besl, McKay
 * A method for registration of 3d Shapes
 * IEEE TPAMI Vol 14, No 2 1992
 */

template < class  S >
void ComputeRigidMatchMatrix(std::vector<Point3<S> > &Pfix,
                             std::vector<Point3<S> > &Pmov,
                             Quaternion<S>  &q,
                             Point3<S>  &tr)
{
  Eigen::Matrix3d ccm;
  Point3<S> bfix,bmov; // baricenter of src e trg

  ComputeCrossCovarianceMatrix(Pmov,bmov,Pfix,bfix,ccm);

  Eigen::Matrix3d cyc; // the cyclic components of the cross covariance matrix.
  cyc=ccm-ccm.transpose();

  Eigen::Matrix4d  QQ;
  QQ.setZero();
  Eigen::Vector3d D(cyc(1,2),cyc(2,0),cyc(0,1));

  Eigen::Matrix3d RM;
  RM.setZero();
  RM(0,0)=-ccm.trace();
  RM(1,1)=-ccm.trace();
  RM(2,2)=-ccm.trace();
  RM += ccm + ccm.transpose();

  QQ(0,0) = ccm.trace();
  QQ.block<1,3> (0,1) = D.transpose();
  QQ.block<3,1> (1,0) = D;
  QQ.block<3,3> (1,1) = RM;

  Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eig(QQ);
  Eigen::Vector4d eval = eig.eigenvalues();
  Eigen::Matrix4d evec = eig.eigenvectors();
//  std::cout << "EigenVectors:" << std::endl << evec << std::endl;
//  std::cout << "Eigenvalues:" << std::endl << eval << std::endl;
  int ind;
  eval.cwiseAbs().maxCoeff(&ind);

  q=Quaternion<S>(evec.col(ind)[0],evec.col(ind)[1],evec.col(ind)[2],evec.col(ind)[3]);
  Matrix44<S> Rot;
  q.ToMatrix(Rot);
  tr= (bfix - Rot*bmov);
}


/*! \brief Compute the roto-translation that applied to PMov bring them onto Pfix
 * Rotation is computed as a quaternion.
 *
 * E.g. it find a matrix such that:
 *
 *       Pfix[i] = res * Pmov[i]
 *
 * Ref:
 * Besl, McKay
 * A method for registration of 3d Shapes
 * IEEE TPAMI Vol 14, No 2 1992
 */

template < class  S >
void ComputeRigidMatchMatrix(std::vector<Point3<S> > &Pfix,
                             std::vector<Point3<S> > &Pmov,
                             Matrix44<S> &res)
{
    Quaternion<S>  q;
    Point3<S>  tr;
    ComputeRigidMatchMatrix(Pfix,Pmov,q,tr);

    Matrix44<S> Rot;
    q.ToMatrix(Rot);

    Matrix44<S> Trn;
    Trn.SetTranslate(tr);

    res=Trn*Rot;
}

/*! \brief Computes the best fitting rigid transformations to align two sets of corresponding points
 *
 * Ref:
 * Olga Sorkine-Hornung and Michael Rabinovich
 * Least-Squares Rigid Motion Using SVD
 */
template <class S>
Matrix44<S> ComputeLeastSquaresRigidMotion(std::vector<Point3<S> > &pFix,
                                           std::vector<Point3<S> > &pMov)
{
	if (pFix.size() != pMov.size() || pFix.size() < 3)
		return Matrix44<S>::Identity();

	Eigen::Matrix3Xd p(3, pMov.size()); // moving
	Eigen::MatrixX3d q(pFix.size(), 3); // fixed

	for (size_t i=0; i<pMov.size(); i++)
	{
		Eigen::Vector3d v;
		pMov[i].ToEigenVector(v);
		p.col(i) = v;
	}
	Eigen::Vector3d avgP = p.rowwise().mean();
	p.colwise() -= avgP;

	for (size_t i=0; i<pFix.size(); i++)
	{
		Eigen::Vector3d v;
		pFix[i].ToEigenVector(v);
		q.row(i) = v;
	}
	Eigen::Vector3d avgQ = q.colwise().mean();
	q.rowwise() -= avgQ.transpose();

	Eigen::Matrix3d cov = p * q;
	Eigen::JacobiSVD<Eigen::Matrix3d> svd;
	svd.compute(cov, Eigen::ComputeFullU | Eigen::ComputeFullV);

	Eigen::Matrix3d d = Eigen::Matrix3d::Identity();
	d(2,2) = (svd.matrixV() * svd.matrixU().transpose()).determinant() > 0 ? 1 : -1;

	Eigen::Matrix3d R = (svd.matrixV() * d * svd.matrixU().transpose());
	Eigen::Vector3d t = avgQ - R * avgP;

	Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
	res.block<3,3>(0,0) = R;
	res.block<3,1>(0,3) = t;
	Matrix44<S> ret;
	ret.FromEigenMatrix(res);
	return ret;
}


/*
Compute a similarity matching (rigid + uniform scaling)
simply create a temporary point set with the correct scaling factor
*/ 
template <class S>
void ComputeSimilarityMatchMatrix(std::vector<Point3<S> > &Pfix,
                                  std::vector<Point3<S> > &Pmov,
                                  Matrix44<S> &res)
{
  S scalingFactor=0;
  for(size_t i=0;i<( Pmov.size()-1);++i)
  {
    scalingFactor += Distance(Pmov[i],Pmov[i+1])/ Distance(Pfix[i],Pfix[i+1]);
  }
  scalingFactor/=(Pmov.size()-1);

  std::vector<Point3<S> > Pnew(Pmov.size());
  for(size_t i=0;i<Pmov.size();++i)
    Pnew[i]=Pmov[i]/scalingFactor;

  ComputeRigidMatchMatrix(Pfix,Pnew,res);

  Matrix44<S> scaleM; scaleM.SetDiagonal(1.0/scalingFactor);
  res = res * scaleM;
}

} // end namespace

#endif