//##########################################################################
//#                                                                        #
//#                              CLOUDCOMPARE                              #
//#                                                                        #
//#  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; version 2 or later of the License.      #
//#                                                                        #
//#  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 for more details.                          #
//#                                                                        #
//#          COPYRIGHT: EDF R&D / TELECOM ParisTech (ENST-TSI)             #
//#                                                                        #
//##########################################################################

#include "ccIndexedTransformation.h"

//Qt
#include <QFile>
#include <QTextStream>

//System
#include <assert.h>
#include <string>

ccIndexedTransformation::ccIndexedTransformation()
	: ccGLMatrix()
	, m_index(0)
{
}

ccIndexedTransformation::ccIndexedTransformation(const ccGLMatrix& matrix)
	: ccGLMatrix(matrix)
	, m_index(0)
{
}

ccIndexedTransformation::ccIndexedTransformation(const ccGLMatrix& matrix, double index)
	: ccGLMatrix(matrix)
	, m_index(index)
{
}

bool ccIndexedTransformation::toAsciiFile(QString filename, int precision/*=12*/) const
{
	QFile fp(filename);
	if (!fp.open(QFile::WriteOnly | QFile::Text))
		return false;

	QTextStream stream(&fp);
	stream.setRealNumberNotation(QTextStream::FixedNotation);
	stream.setRealNumberPrecision(precision);

	//save transformation first (so that it can be loaded as a ccGLMatrix!)
	for (unsigned i=0; i<4; ++i)
	{
		stream << m_mat[i] << " " << m_mat[i+4] << " " << m_mat[i+8] << " " << m_mat[i+12] << endl;
	}
	//save index next
	stream << m_index;

	return (fp.error() == QFile::NoError);
}

bool ccIndexedTransformation::fromAsciiFile(QString filename)
{
	QFile fp(filename);
	if (!fp.open(QFile::ReadOnly | QFile::Text))
		return false;

	QTextStream stream(&fp);

	//read the transformation first
	for (unsigned i=0; i<4; ++i)
	{
		stream >> m_mat[i];
		stream >> m_mat[i+4];
		stream >> m_mat[i+8];
		stream >> m_mat[i+12];
	}
	//read index
	stream >> m_index;

	return (fp.error() == QFile::NoError);
}

ccIndexedTransformation ccIndexedTransformation::operator * (const ccGLMatrix& M) const
{
	return ccIndexedTransformation( *static_cast<const ccGLMatrix*>(this) * M, m_index);
}

ccIndexedTransformation& ccIndexedTransformation::operator *= (const ccGLMatrix& M)
{
	ccGLMatrix temp = (*this) * M;
	(*this) = temp;

	return (*this);
}

//ccIndexedTransformation ccIndexedTransformation::operator * (const ccIndexedTransformation& trans) const
//{
//	return ccIndexedTransformation( static_cast<ccGLMatrix*>(this) * trans, m_index);
//}
//
//ccIndexedTransformation& ccIndexedTransformation::operator *= (const ccIndexedTransformation& trans)
//{
//	ccGLMatrix temp = (*this) * M;
//	(*this) = temp;
//
//	return (*this);
//}

ccIndexedTransformation& ccIndexedTransformation::operator += (const CCVector3& T)
{
	*static_cast<ccGLMatrix*>(this) += T;

	return (*this);
}

ccIndexedTransformation& ccIndexedTransformation::operator -= (const CCVector3& T)
{
	*static_cast<ccGLMatrix*>(this) -= T;

	return (*this);
}

ccIndexedTransformation ccIndexedTransformation::transposed() const
{
	ccIndexedTransformation t(*this);
	t.transpose();
	
	return t;
}

ccIndexedTransformation ccIndexedTransformation::inverse() const
{
	ccIndexedTransformation t(*this);
	t.invert();

	return t;
}

ccIndexedTransformation ccIndexedTransformation::Interpolate(	double index,
																const ccIndexedTransformation& trans1,
																const ccIndexedTransformation& trans2)
{
	double dt = trans2.getIndex() - trans1.getIndex();
	if (dt == 0.0)
	{
		assert(index == trans1.getIndex());
		return trans1;
	}

	//we compute the transformation matrix between trans1 and trans2
	double t = (index - trans1.getIndex())/dt;
	assert(t >= 0 && t <= 1);
	
	ccGLMatrix mat = ccGLMatrix::Interpolate(static_cast<PointCoordinateType>(t),trans1,trans2);

	return ccIndexedTransformation(mat, index);
}

bool ccIndexedTransformation::toFile(QFile& out, short dataVersion) const
{
	assert(out.isOpen() && (out.openMode() & QIODevice::WriteOnly));
	if (dataVersion < 34)
	{
		assert(false);
		return false;
	}

	if (!ccGLMatrix::toFile(out, dataVersion))
		return false;

	assert(out.isOpen() && (out.openMode() & QIODevice::WriteOnly));

	//index (dataVersion>=34)
	if (out.write((const char*)&m_index, sizeof(double)) < 0)
		return WriteError();

	return true;
}

bool ccIndexedTransformation::fromFile(QFile& in, short dataVersion, int flags, LoadedIDMap& oldToNewIDMap)
{
	if (!ccGLMatrix::fromFile(in, dataVersion, flags, oldToNewIDMap))
		return false;

	assert(in.isOpen() && (in.openMode() & QIODevice::ReadOnly));

	if (dataVersion < 34)
		return CorruptError();

	//index (dataVersion>=34)
	if (in.read((char*)&m_index, sizeof(double)) < 0)
		return ReadError();

	return true;
}

short ccIndexedTransformation::minimumFileVersion() const
{
	return std::max(static_cast<short>(34), ccGLMatrix::minimumFileVersion());
}

