#include "TestTransform.h"
#include <QTransform>
#include <QPainter>
#include <opencv.hpp>
#include <vector>

using namespace cv;
using namespace std;


#define M_LOW_TOLERANCE 0.000001
void transPoints(vector<Point2d>& vec, const Matx33d& mat)
{
	for (size_t i = 0; i < vec.size(); ++i)
	{
		Point2d p = vec[i];
		Point3d tp = mat * p;
		vec[i] = Point2d(tp.x / tp.z, tp.y / tp.z);
	}
}

void transPoints(vector<Point2d>& vec, const Mat& mat)
{
	Matx33d matx = Matx33d::eye();
	Mat matx_(3, 3, CV_64FC1, matx.val);

	if (mat.rows == 2 && mat.cols == 3)
	{
		mat.copyTo(matx_.rowRange(0, 2));
	}
	else if (mat.rows == 3 && mat.cols == 3)
	{
		mat.copyTo(matx_);
	}
	else
	{
		std::cout << "not supported transformation mat with its size as " \
			<< mat.rows << "x" << mat.cols << std::endl;
		return;
	}
	transPoints(vec, matx);
}

Matx33d affineTrans(const vector<Point2d>& src, const vector<Point2d>& dst)
{
	if (dst.empty() || src.empty()) {
		return Mat();
	}

	Point2d pc, qc;
	int smallerSize = src.size() < dst.size() ? src.size() : dst.size();

	for (int i = 0; i < smallerSize; i++) {
		pc += src[i];
		qc += dst[i];
	}
	pc.x /= smallerSize;
	pc.y /= smallerSize;
	qc.x /= smallerSize;
	qc.y /= smallerSize;

	Matx21d pit;
	Matx12d pi;
	Matx12d qi;
	Matx22d spitpi = Matx22d::zeros();
	Matx22d pitpi;
	Matx22d pitqi;
	Matx22d spitqi = Matx22d::zeros();

	for (int i = 0; i < src.size() && i < dst.size(); i++) {
		Point2d qpi = src[i] - pc;
		Point2d qqi = dst[i] - qc;

		pit(0) = qpi.x;
		pit(1) = qpi.y;
		pi(0) = qpi.x;
		pi(1) = qpi.y;
		qi(0) = qqi.x;
		qi(1) = qqi.y;
		pitpi = pit * pi;
		spitpi = pitpi + spitpi;
		pitqi = pit * qi;
		spitqi = pitqi + spitqi;
	}
	Matx22d ispitpi;
	ispitpi = spitpi.inv();

	Matx22d M = ispitpi * spitqi;

	double m11 = M(0, 0);
	double m21 = M(0, 1);
	double m12 = M(1, 0);
	double m22 = M(1, 1);

	Matx33d qm(m11, m12, 0, m21, m22, 0, 0, 0, 1);
	Matx33d pcm(1.0, 0, -pc.x, 0, 1.0, -pc.y, 0, 0, 1);
	Matx33d qcm(1.0, 0, qc.x, 0, 1.0, qc.y, 0, 0, 1);

	Matx33d ret = qcm * qm*pcm;

	return ret;
}

cv::Matx33d rigidTrans(const vector<Point2d>& src, const vector<Point2d>& dst,
	Mat* pCenRotScaleMat = NULL)
{
	if (dst.empty() || src.empty()) {
		return Mat();
	}

	vector<double> weights(src.size(), 1.0 / src.size());
	Point2d pc, qc;
	double wsum = 0;

	for (int i = 0; i < src.size(); i++) {
		double w = 1.0 / src.size();
		weights[i] = w;
		pc += src[i] * w;
		qc += dst[i] * w;
		wsum += w;
	}
	pc.x /= wsum;
	pc.y /= wsum;
	qc.x /= wsum;
	qc.y /= wsum;

	double u = 0;
	double u1, u2;
	u1 = 0;
	u2 = 0;
	for (int i = 0; i < src.size() && i < dst.size(); i++) {
		Point2d qpi = src[i] - pc;
		Point2d qqi = dst[i] - qc;
		Point2d pi(qpi.x, qpi.y);
		Point2d qi(qqi.x, qqi.y);
		u1 += pi.dot(qi)*weights[i];
		Point2d pi_(pi.y, -pi.x);
		u2 += qi.dot(pi_)*weights[i];
	}
	u = sqrt(u1*u1 + u2 * u2);
	if (u < M_LOW_TOLERANCE) {
		u = M_LOW_TOLERANCE;
	}

	Matx22d R = Matx22d::zeros();
	Matx22d r = Matx22d::zeros();

	for (int i = 0; i < src.size() && i < dst.size(); i++) {
		Point2d qpi = src[i] - pc;
		Point2d qqi = dst[i] - qc;
		Point2d pi(qpi.x, qpi.y);
		Point2d qi(qqi.x, qqi.y);
		Point2d pi_(pi.y, -pi.x);
		Point2d qi_(qi.y, -qi.x);

		r(0, 0) = pi.dot(qi);
		r(0, 1) = pi.dot(qi_);
		r(1, 0) = pi_.dot(qi);
		r(1, 1) = pi_.dot(qi_);

		R = R + r * (weights[i] / u);
	}

	double m11 = R(0, 0);
	double m21 = R(0, 1);
	double m12 = R(1, 0);
	double m22 = R(1, 1);
	Matx33d qm(m11, m12, 0, m21, m22, 0, 0, 0, 1);
	Matx33d pcm(1.0, 0, -pc.x, 0, 1.0, -pc.y, 0, 0, 1);
	Matx33d qcm(1.0, 0, qc.x, 0, 1.0, qc.y, 0, 0, 1);
	Matx33d ret = qcm * qm*pcm;

	if (pCenRotScaleMat)
	{
		*pCenRotScaleMat = Mat(qm);
	}

	return ret;
}

bool cmpPointVec(const vector<Point2d>& vec0, const vector<Point2d>& vec1, const Matx33d& mat, double tor)
{
	int smallerSize = vec0.size() < vec1.size() ? vec0.size() : vec1.size();
	for (int i = 0; i < smallerSize; ++i)
	{
		Point2d p0, p1;
		p0 = vec0[i];
		p1 = vec1[i];
		Point3d tp0 = mat * p0;
		tp0.x /= tp0.z;
		tp0.y /= tp0.z;
		if (abs(p1.x - tp0.x) > tor || abs(p1.y - tp0.y) > tor)
		{
			return false;
		}
	}
	return true;
}

void testTransSolver()
{
	{
		//rotation only

		vector<Point2d> vec0, vec1;
		vec0.push_back(Point2d());
		vec0.push_back(Point2d(1, 0));
		vec0.push_back(Point2d(1, 1));
		vec0.push_back(Point2d(0, 1));

		vec1.push_back(Point2d());
		vec1.push_back(Point2d(0, 1));
		vec1.push_back(Point2d(-1, 1));
		vec1.push_back(Point2d(-1, 0));

		Matx33d ret = affineTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));

		ret = rigidTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
	}
	{
		// rotation and scale

		vector<Point2d> vec0, vec1;
		vec0.push_back(Point2d());
		vec0.push_back(Point2d(1, 0));
		vec0.push_back(Point2d(1, 1));
		vec0.push_back(Point2d(0, 1));

		vec1.push_back(Point2d());
		vec1.push_back(Point2d(1, 1));
		vec1.push_back(Point2d(0, 2));
		vec1.push_back(Point2d(-1, 1));

		Matx33d ret = affineTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
	}
	{
		// scale only

		vector<Point2d> vec0, vec1;
		vec0.push_back(Point2d());
		vec0.push_back(Point2d(1, 0));
		vec0.push_back(Point2d(1, 1));
		vec0.push_back(Point2d(0, 1));

		vec1.push_back(Point2d());
		vec1.push_back(Point2d(2, 0));
		vec1.push_back(Point2d(2, 2));
		vec1.push_back(Point2d(0, 2));

		Matx33d ret = affineTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
	}
	{
		// translation only

		vector<Point2d> vec0, vec1;
		vec0.push_back(Point2d());
		vec0.push_back(Point2d(1, 0));
		vec0.push_back(Point2d(1, 1));
		vec0.push_back(Point2d(0, 1));

		vec1.push_back(Point2d(1, 1));
		vec1.push_back(Point2d(2, 1));
		vec1.push_back(Point2d(2, 2));
		vec1.push_back(Point2d(1, 2));

		Matx33d ret = affineTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));

		ret = rigidTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
	}
	{
		for (int i = 0; i < 10000; ++i)
		{
			// random rotation and translation
			Mat mat23 = getRotationMatrix2D(Point2f(), rand() % 360, 1.0);
			mat23.at<double>(0, 2) = (rand() % 1000) / 1000.0;
			mat23.at<double>(1, 2) = (rand() % 1000) / 1000.0;
			Matx33d matx = Matx33d::eye();
			Mat mat(3, 3, CV_64FC1, matx.val);
			mat23.copyTo(mat.rowRange(0, 2));

			vector<Point2d> vec0, vec1;
			vec0.push_back(Point2d());
			vec0.push_back(Point2d(1, 0));
			vec0.push_back(Point2d(1, 1));
			vec0.push_back(Point2d(0, 1));

			vec1 = vec0;
			transPoints(vec1, matx);

			Matx33d ret = affineTrans(vec0, vec1);
			_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
			ret = rigidTrans(vec0, vec1);
			_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
		}
	}
	{
		// skew only

		vector<Point2d> vec0, vec1;
		vec0.push_back(Point2d());
		vec0.push_back(Point2d(1, 0));
		vec0.push_back(Point2d(1, 1));
		vec0.push_back(Point2d(0, 1));

		vec1.push_back(Point2d());
		vec1.push_back(Point2d(1, 0));
		vec1.push_back(Point2d(2, 1));
		vec1.push_back(Point2d(1, 1));

		Matx33d ret = affineTrans(vec0, vec1);
		_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
	}
	{
		// random affine

		for (int i = 0; i < 10000; ++i)
		{
			vector<Point2d> vec0, vec1;
			vec0.push_back(Point2d());
			vec0.push_back(Point2d(1, 0));
			vec0.push_back(Point2d(1, 1));
			vec0.push_back(Point2d(0, 1));

			Matx33d trans = Matx33d::eye();
			Mat mat(3, 3, CV_64FC1, trans.val);
			randu(mat.rowRange(0, 2), 0, 1.0);

			vec1 = vec0;
			transPoints(vec1, trans);

			Matx33d ret = affineTrans(vec0, vec1);
			_ASSERTE(cmpPointVec(vec0, vec1, ret, M_LOW_TOLERANCE));
		}
	}
}


void translate(QPolygonF srcPos, QPolygonF dstPos)
{
	
	vector<Point2d> vec0, vec1;
	vec0.push_back(Point2d(srcPos.at(0).x(), srcPos.at(0).y()));
	vec0.push_back(Point2d(srcPos.at(1).x(), srcPos.at(1).y()));
	vec0.push_back(Point2d(srcPos.at(2).x(), srcPos.at(2).y()));
	vec0.push_back(Point2d(srcPos.at(3).x(), srcPos.at(3).y()));

	vec1.push_back(Point2d(dstPos.at(0).x(), dstPos.at(0).y()));
	vec1.push_back(Point2d(dstPos.at(1).x(), dstPos.at(1).y()));
	vec1.push_back(Point2d(dstPos.at(2).x(), dstPos.at(2).y()));
	vec1.push_back(Point2d(dstPos.at(3).x(), dstPos.at(3).y()));
	Matx33d ret = affineTrans(vec0, vec1);
	Mat retMat = Mat(ret);



	Point2f srcVec[4];
	Point2f dstTri[4];

	srcVec[0].x = srcPos.at(0).x();
	srcVec[0].y = srcPos.at(0).y();

	srcVec[1].x = srcPos.at(1).x();
	srcVec[1].y = srcPos.at(1).y();

	srcVec[2].x = srcPos.at(2).x();
	srcVec[2].y = srcPos.at(2).y();

	srcVec[3].x = srcPos.at(3).x();
	srcVec[3].y = srcPos.at(3).y();

	dstTri[0].x = dstPos.at(0).x();
	dstTri[0].y = dstPos.at(0).y();

	dstTri[1].x = dstPos.at(1).x();
	dstTri[1].y = dstPos.at(1).y();

	dstTri[2].x = dstPos.at(2).x();
	dstTri[2].y = dstPos.at(2).y();

	dstTri[3].x = dstPos.at(3).x();
	dstTri[3].y = dstPos.at(3).y();

	


	/// ÇóµÃ·ÂÉä±ä»»
	Mat warp_mat = getAffineTransform(srcVec, dstTri);
	double* pTransMat = (double*)(warp_mat.data);
	double m00=pTransMat[0];
	double m10=pTransMat[1];
	double m20=pTransMat[2];
	double m01=pTransMat[3];
	double m11=pTransMat[4];
	double m21=pTransMat[5];
	
	int newX = 100;
	int newY = 100;

	double temX = newX * m00 + newY * m10 + m20;
	double temY = newX * m01 + newY * m11 + m21;
	int a = 0;
}


TestTransform::TestTransform(QWidget *parent)
	: QMainWindow(parent)
{
	ui.setupUi(this);
	connect(ui.pushButton, SIGNAL(clicked()), this, SLOT(onButtonClicked()));
}

Q_SLOT void TestTransform::onButtonClicked()
{
	//testTransSolver();
	QPolygonF srcP;
	srcP.append(QPointF(-105, -100));
	srcP.append(QPointF(-204, -100));
	srcP.append(QPointF(-300, -200));
	srcP.append(QPointF(-200, -200));

	QPolygonF dstP;
	dstP.append(QPointF(-50, 50));
	dstP.append(QPointF(50, 50));
	dstP.append(QPointF(50, -50));
	dstP.append(QPointF(-50, -50));
	translate(srcP, dstP);

}

void TestTransform::paintEvent(QPaintEvent *event)
{
	QPainter painter(this);
	QFont font("Courier", 24);
	painter.setFont(font);
	painter.drawText(100, 100, "Hello, world!");
	QTransform transform;
	transform.rotate(15.0);
	painter.setWorldTransform(transform);
	painter.drawText(100, 100, "Hello, world!");
}