#include "global.h"

#include <qpainter.h>
#include <qmessagebox.h>
#include <math.h>

#include "rectifydialog.h"
#include "rectification.h"
#include "g12Image.h"

RectifyDialog::RectifyDialog(QWidget *parent)
    : QWidget(parent)
{
	ui.setupUi(this);

	/* Main UI*/
	connect(ui.widget, SIGNAL(askParentRepaint(QPaintEvent *, QWidget *)), this, SLOT(repaintChild(QPaintEvent *, QWidget *)));
	connect(ui.widget, SIGNAL(notifyParentMouseMove(QMouseEvent* )), this, SLOT(mouseChild(QMouseEvent *)));

	connect(ui.leftBufferButton, SIGNAL(released()),  ui.widget, SLOT(update()));
	connect(ui.rightBufferButton, SIGNAL(released()),  ui.widget, SLOT(update()));
	connect(ui.showRectifiedBox, SIGNAL(stateChanged(int)),  ui.widget, SLOT(update()));


	list = NULL;
	buffer1 = NULL;
	buffer2 = NULL;

	leftTransform = getScale2D(1,1);
	rightTransform = getScale2D(1,1);

	buffer1Transformed = NULL;
	buffer2Transformed = NULL;

	corrNumber = 0;
	F = getScale2D(1,1);

	ui.fundamentalMatrix->setProjectiveTransform(&F);

	ui.ransacIterBox->setMaximum(10000);
	ui.ransacIterBox->setMinimum(20);
	ui.ransacIterBox->setValue(100);

	ui.ransacTestSizeBox->setMaximum(100);
	ui.ransacTestSizeBox->setMinimum(10);
	ui.ransacTestSizeBox->setValue(11);

	ui.ransacThresBox->setMaximum(10);
	ui.ransacThresBox->setMinimum(0.001);
	ui.ransacThresBox->setValue(1.0);

	ui.autoShiftBox->setChecked(true);

	ui.shiftBox->setMaximum(1000);
	ui.shiftBox->setMinimum(-1000);

	connect(ui.recalculateButton, SIGNAL(released()), this, SLOT(recalculate()));

	z.x = 1.0;
	z.y = 0.0;
	z.z = 0.0;

	ui.directionLabel->setText(QString("%1, %2").arg(z.x,3,'f',3).arg(z.y,3,'f',3));
	connect(ui.diectionDial, SIGNAL(valueChanged(int)), this, SLOT(dialMovedSlot(int)));
	ui.recalculateButton->setEnabled(false);
}

void RectifyDialog::dialMovedSlot(int i)
{
	double direction = ((double) ui.diectionDial->value() / ui.diectionDial->maximum()) * 2 * M_PI;
	z.x = cos(direction);
	z.y = sin(direction);
	ui.directionLabel->setText(QString("%1, %2").arg(z.x,3,'f',3).arg(z.y,3,'f',3));
}


void RectifyDialog::corrListChanged(Correspondance *corr, int coornumber)
{
	this->list = corr;
	this->corrNumber = coornumber;
	ui.recalculateButton->setEnabled(true);
	this->update();
}

void RectifyDialog::buffersChangedSlot(G12Buffer *buffer1, G12Buffer *buffer2)
{
	this->buffer1 = buffer1;
	this->buffer2 = buffer2;
	this->list = NULL;
	ui.recalculateButton->setEnabled(false);
	this->update();
}


void RectifyDialog::recalculate(void)
{
	//emit matchNeeded();
	if (list == NULL)
	{
		QMessageBox::information(
				this,
				"No matcher information",
				QString("Could not rectificate. No matches are generated.\n Go to viMatch to generate correspondences"),
				"OK");
		return;
	}

	int i;
	int trySize = ui.ransacTestSizeBox->value();
	int iterations = ui.ransacIterBox->value();
	double threshold = ui.ransacThresBox->value();

	projectiveTransform2D leftNormalize;
	projectiveTransform2D rightNormalize;

	double scale = getNormalisingTransformation(list, corrNumber, &leftNormalize, &rightNormalize);

	Correspondance *corrNorm = (Correspondance *) dv_malloc(corrNumber * sizeof(Correspondance));
	memcpy(corrNorm, list, corrNumber * sizeof(Correspondance));

	// Get the fundamental matrix for the corrected point list
	transformCorrespondence(corrNorm, corrNumber, &leftNormalize, &rightNormalize);
	Matrix *Fmatrix = rectifyRANSAC(corrNorm, corrNumber, trySize, iterations, scale * threshold);
	projectiveTransform2D F = projective2DCreateFromMatrix(Fmatrix);
	matrixDelete(Fmatrix);

	/*Coping the inliers information*/
	for (i = 0; i < corrNumber; i++)
	{
		list[i].flags = corrNorm[i].flags;
	}

	// Get the fundamental matrix for the real point list
	F = multiplyBy2D(projective2Dtranspose(leftNormalize), multiplyBy2D(F, rightNormalize));
	printf("Global Fundamental Matrix:\n");
	projective2Dprint(&F);
	ui.fundamentalMatrix->setProjectiveTransform(&F);

	// Find epipoles
	vector3Dd E1, E2;
	findEpipole(&F, &E1, &E2);
	ui.epipole1Label->setText(
			QString("Epipole 1 [%1:%2:%3]")
			.arg(E1.x, 3, 'f', 3)
			.arg(E1.y, 3, 'f', 3)
			.arg(E1.z, 3, 'f', 3));

	ui.epipole2Label->setText(
			QString("Epipole 2 [%1:%2:%3]")
			.arg(E2.x, 3, 'f', 3)
			.arg(E2.y, 3, 'f', 3)
			.arg(E2.z, 3, 'f', 3));


	// Get rectification transformation
        getRectificationTransformationOptimal(&F, corrNorm, corrNumber, &rightTransform, &leftTransform, z);

	vector2Dd center = {buffer1->w / 2, buffer1->h / 2};
	vector2Dd centerShift = projective2DapplyToCartesian(&leftTransform, center);
	projectiveTransform2D backShift = getShift2D( - centerShift.x + center.x, - centerShift.y + center.y);

	leftTransform = multiplyBy2D(backShift, leftTransform);
	rightTransform = multiplyBy2D(backShift, rightTransform);


	double min;
	double max;
	if (ui.autoShiftBox->isChecked())
	{
		getLateralCorrection(list, corrNumber, &leftTransform, &rightTransform, &min, &max);
	} else
	{
		max = ui.shiftBox->value();
		min = ui.shiftBox->value() - 100;
	}

	leftTransform = multiplyBy2D(getShift2D(max / 2, 0), leftTransform);
	rightTransform = multiplyBy2D(getShift2D(-max / 2, 0), rightTransform);


	projectiveTransform2D inverseLeft = projective2Dinverse(&leftTransform);
	projectiveTransform2D inverseRight = projective2Dinverse(&rightTransform);

	buffer1Transformed = applyAffineTransformBilinear(inverseLeft, buffer1, buffer1->h, buffer1->w);
	buffer2Transformed = applyAffineTransformBilinear(inverseRight, buffer2, buffer2->h, buffer2->w);

	emit buffersChanged(buffer1Transformed, buffer2Transformed);
	emit maxDisparityChanged(200);
	this->update();

}

void RectifyDialog::repaintChild(QPaintEvent *event, QWidget *who)
{
	QPainter painter(who);

	if (!ui.showRectifiedBox->isChecked())
	{
		if (ui.leftBufferButton->isChecked() && buffer1 != NULL)
		{
			g12Image image(buffer1);
			painter.drawImage(QPoint(0,0), image);
		}

		if (ui.rightBufferButton->isChecked() && buffer2 != NULL)
		{
			g12Image image(buffer2);
			painter.drawImage(QPoint(0,0), image);
		}

		if (buffer1 == NULL)
			painter.drawText(0,0, who->width()/2, who->height(), Qt::AlignHCenter | Qt::AlignVCenter, QString("NO LEFT IMAGE"));
		if (buffer2 == NULL)
			painter.drawText(who->width()/2, 0, who->width()/2, who->height(), Qt::AlignHCenter | Qt::AlignVCenter, QString("NO RIGHT IMAGE"));

		if (ui.pointsBox->isChecked())
		{
			int i;
			for (i = 0; i < corrNumber; i++)
			{
				vector2Dd start = list[i].start;
				vector2Dd end   = list[i].end;

				if (list[i].flags)
					painter.setPen(Qt::green);
				else
					painter.setPen(Qt::red);

				painter.drawLine(start.x, start.y, end.x, end.y);

				if (ui.leftBufferButton->isChecked())
					painter.drawRect(start.x - 1, start.y - 1, 3 , 3);
				else
					painter.drawRect(end.x - 1, end.y - 1, 3 , 3);
			}

		}

	} else {
		if (ui.leftBufferButton->isChecked() && buffer1Transformed != NULL)
		{
			g12Image image(buffer1Transformed);
			painter.drawImage(QPoint(0,0), image);
		}

		if (ui.rightBufferButton->isChecked() && buffer2Transformed != NULL)
		{
			g12Image image(buffer2Transformed);
			painter.drawImage(QPoint(0,0), image);
		}

		if (buffer1Transformed == NULL)
			painter.drawText(0,0, who->width()/2, who->height(), Qt::AlignHCenter | Qt::AlignVCenter, QString("NO LEFT RECTIFIED IMAGE"));
		if (buffer2Transformed == NULL)
			painter.drawText(who->width()/2, 0, who->width()/2, who->height(), Qt::AlignHCenter | Qt::AlignVCenter, QString("NO RIGHT RECTIFIED IMAGE"));


		if (ui.pointsBox->isChecked())
		{
			int i;
			for (i = 0; i < corrNumber; i++)
			{
				vector2Dd start = projective2DapplyToCartesian(&leftTransform, list[i].start);
				vector2Dd end   = projective2DapplyToCartesian(&rightTransform, list[i].end);

				if (list[i].flags)
					painter.setPen(Qt::green);
				else
					painter.setPen(Qt::red);

				painter.drawLine(start.x, start.y, end.x, end.y);

				if (ui.leftBufferButton->isChecked())
					painter.drawRect(start.x - 1, start.y - 1, 3 , 3);
				else
					painter.drawRect(end.x - 1, end.y - 1, 3 , 3);
			}

		}

	}





}

void RectifyDialog::mouseChild(QMouseEvent *event)
{

}

RectifyDialog::~RectifyDialog()
{

}
