//==============================================================================
/*! \file
 * OpenMesh Toolkit for mesh analysis    \n
 * Copyright (c) 2010 by Rostislav Hulik     \n
 *
 * Author:  Rostislav Hulik, rosta.hulik@gmail.com  \n
 * Date:    2010/11/21                          \n
 *
 * This file is part of software developed for support of Rostislav Hulik's dissertation thesis at dcgm-robotics@FIT group.
 *
 * This file is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This file 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 Lesser General Public License for more details.
 * 
 * You should have received a copy of the GNU Lesser General Public License
 * along with this file.  If not, see <http://www.gnu.org/licenses/>.
 * 
 * Description:
 * - Class computig transformation between mesh and raster which is positioned on a vertex and is tangent to surface
 */

#ifndef _OM_TRANSFORMATION_SOLVER_HXX_
#define _OM_TRANSFORMATION_SOLVER_HXX_

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Constructor - computes desired transformation
// Produces transformation matrix, which projects mesh coordinates intu tangent raster coordinates
// Center vertex will be projected to a center of a matrix
// @param matrixLength Raster length - square raster edge length (real in mesh space)
// @param resolution Number of raster pixels (matrix dimensions) - matrix is squared
// @param normal Normal on spacified vertex (futire Z direction)
// @param direction Direction of future X direction (for example maximum curvature direction)
// @param origin Coordinates of a vertex
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
OMTransformationSolver<Vector>::OMTransformationSolver(ScalarT matrixLength, ScalarT resolution, VectorT normal, VectorT direction, VectorT origin)
{
	// init helper variables
	m_matrixLength = matrixLength;
	m_resolution = resolution;
	m_pixelSize = m_resolution/m_matrixLength;
	ScalarT halfSize = m_resolution/2-0.5;

	// shift to center
	m_modelTo2D = Eigen::Translation<ScalarT, 3>(-origin[0], -origin[1], -origin[2]);

	// set aux to normal orthogonaly projected to XY plane
	VectorT aux = normal;
	aux[2] = 0.0;
		
	// Align with ZX plane
	ScalarT angle = vectorSignedAngle(VectorT(1.0, 0.0, 0.0), aux, VectorT(0.0, 0.0, 1.0));
	m_modelTo2D = AngleAxis<ScalarT>(-angle, VectorEigen::UnitZ()) * m_modelTo2D;

	// update aux
	aux = transformTo2DLinear(normal);
    
	// Align with ZY plane
	angle = vectorSignedAngle(VectorT(0.0, 0.0, 1.0), aux, VectorT(0.0, 1.0, 0.0));
	m_modelTo2D = AngleAxis<ScalarT>(-angle, VectorEigen::UnitY()) * m_modelTo2D;

	// Rotate ZX plane to satisfy direction
	aux = transformTo2DLinear(direction);
	aux[2] = 0.0;
	angle = vectorSignedAngle(VectorT(1.0, 0.0, 0.0), aux, VectorT(0.0, 0.0, 1.0));
	m_modelTo2D = AngleAxis<ScalarT>(-angle, VectorEigen::UnitZ()) * m_modelTo2D;

	// scale
	m_modelTo2D = Scaling<ScalarT, 3>(m_pixelSize, m_pixelSize, m_pixelSize)* m_modelTo2D;

	// shift center to center of a matrix
	m_modelTo2D = Translation<ScalarT, 3>(halfSize, halfSize, 0.0) *m_modelTo2D;

	// save inverse matrix
	m_2DToModel = m_modelTo2D.inverse();
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Transforms a mesh point (OpenMesh VecXX) to a raster
// @param point Point to be transformed
// @return Transformed point
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::VectorT OMTransformationSolver<Vector>::transformTo2D(VectorT point)
{
	VectorT aux;
	ScalarT *data = point.data();
	ScalarT *matrixData = m_modelTo2D.data();
	aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8] + matrixData[12];
	aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9] + matrixData[13];
	aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10] + matrixData[14];
	return aux;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Transforms a raster point (OpenMesh VecXX) to a mesh space
// @param point Point to be transformed
// @return Transformed point
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::VectorT OMTransformationSolver<Vector>::transformToMesh(VectorT point)
{
	VectorT aux;
	ScalarT *data = point.data();
	ScalarT *matrixData = m_2DToModel.data();
	aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8] + matrixData[12];
	aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9] + matrixData[13];
	aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10] + matrixData[14];
	return aux;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Transforms a mesh vector (OpenMesh VecXX) to a raster
// @param point Vector to be transformed
// @return Transformed vector
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::VectorT OMTransformationSolver<Vector>::transformTo2DLinear(VectorT vector)
{
	VectorT aux;
	ScalarT *data = vector.data();
	ScalarT *matrixData = m_modelTo2D.data();
	aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8];
	aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9];
	aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10];
	return aux;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Transforms a raster vector (OpenMesh VecXX) to a mesh space
// @param point Vector to be transformed
// @return Transformed vector
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::VectorT OMTransformationSolver<Vector>::transformToMeshLinear(VectorT vector)
{
	VectorT aux;
	ScalarT *data = vector.data();
	ScalarT *matrixData = m_2DToModel.data();
	aux[0] = data[0] * matrixData[0] + data[1] * matrixData[4] + data[2] * matrixData[8];
	aux[1] = data[0] * matrixData[1] + data[1] * matrixData[5] + data[2] * matrixData[9];
	aux[2] = data[0] * matrixData[2] + data[1] * matrixData[6] + data[2] * matrixData[10];
	return aux;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function returns a signed angle (0..Pi) with use of a reference vector
// @param vec1 First vector
// @param vec2 Second vector
// @reference Reference vector (i.e. normal vector etc..)
// @return Signed angle of two vectors in interval 0..Pi
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::ScalarT OMTransformationSolver<Vector>::vectorSignedAngle(VectorT& vec1, VectorT& vec2, VectorT reference)
{
	// calculate vectors length
	ScalarT vec1Length = vec1.norm();
    ScalarT vec2Length = vec2.norm();

    // calculatce vectors angle, test existence
    if ( (vec1Length != 0.0) && (vec2Length != 0.0) )
    {
		VectorT normal;             // given vectors normal

        // calculate vectors normal
		normal = vec1%vec2;
        // determine angle sign
        ScalarT sign = ((normal|reference) < 0) ? -1 : 1; 
        // return result angle
		return acos( (vec1|vec2) / (vec1Length * vec2Length) ) * sign;
    }
    else
       return 0.0;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function returns an unsigned angle (0..Pi)
// @param vec1 First vector
// @param vec2 Second vector
// @return Unsigned angle of two vectors in interval 0..Pi
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::ScalarT OMTransformationSolver<Vector>::vectorAngle(VectorT& vec1, VectorT& vec2)
{
	// calculate vectors length
    ScalarT vec1Length = vec1.norm();
    ScalarT vec2Length = vec2.norm();

    // calculatce vectors angle, test existence
    if ( (vec1Length != 0.0) && (vec2Length != 0.0) )
		return acos( (vec1|vec2) / (vec1Length * vec2Length) );
    else
        return 0.0;
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function returns maximum coordinate of a associed matrix
// @returns Desired dimensions
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::VectorT OMTransformationSolver<Vector>::getMaxBounds()
{
	return VectorT(m_resolution-1, m_resolution-1, 0.0);
}

//////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Function returns pixel width (in a mesh space)
//////////////////////////////////////////////////////////////////////////////////////////////////////////////
template <class Vector>
typename OMTransformationSolver<Vector>::ScalarT OMTransformationSolver<Vector>::getPixelSize()
{
	return m_pixelSize;
}

#endif