//==============================================================================
/*! \file
 * OpenMesh Toolkit for mesh analysis    \n
 * Copyright (c) 2010 by Rostislav Hulik     \n
 *
 * Author:  Rostislav Hulik, rosta.hulik@gmail.com  \n
 * Date:    2010/10/20                          \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 computing maximum curvature from Class for simulating Gauss function
 */
 
#include <OMToolkit/OMMatrixCurvature.h>
#include <OMToolkit\OMTransformationSolver.h>
//#include <Eigen\Core>
#include <MDSTk\Math\mdsMatrix.h>
#include <Eigen\Core>
#include <Eigen\LU>
#include <Eigen\QR>
#include <opencv2\opencv.hpp>
#include <opencv2\core\core.hpp>
#include <opencv2\imgproc\imgproc_c.h>

using namespace cv;

////////////////////////////////////////////////////////////////////////////////
// Taken from OpenCV
////////////////////////////////////////////////////////////////////////////////
static void
calcEigenValsVecs( const Mat& _cov, Mat& _dst )
{
    int i, j;
    Size size = _cov.size();
    if( _cov.isContinuous() && _dst.isContinuous() )
    {
        size.width *= size.height;
        size.height = 1;
    }

    for( i = 0; i < size.height; i++ )
    {
        const float* cov = (const float*)(_cov.data + _cov.step*i);
        float* dst = (float*)(_dst.data + _dst.step*i);

        for( j = 0; j < size.width; j++ )
        {
            double a = cov[j*3];
            double b = cov[j*3+1];
            double c = cov[j*3+2];

            double u = (a + c)*0.5;
            double v = std::sqrt((a - c)*(a - c)*0.25 + b*b);
            double l1 = u + v;
            double l2 = u - v;

            double x = b;
            double y = l1 - a;
            double e = fabs(x);

            if( e + fabs(y) < 1e-4 )
            {
                y = b;
                x = l1 - c;
                e = fabs(x);
                if( e + fabs(y) < 1e-4 )
                {
                    e = 1./(e + fabs(y) + FLT_EPSILON);
                    x *= e, y *= e;
                }
            }

            double d = 1./std::sqrt(x*x + y*y + DBL_EPSILON);
            dst[6*j] = (float)l1;
            dst[6*j + 2] = (float)(x*d);
            dst[6*j + 3] = (float)(y*d);

            x = b;
            y = l2 - a;
            e = fabs(x);

            if( e + fabs(y) < 1e-4 )
            {
                y = b;
                x = l2 - c;
                e = fabs(x);
                if( e + fabs(y) < 1e-4 )
                {
                    e = 1./(e + fabs(y) + FLT_EPSILON);
                    x *= e, y *= e;
                }
            }

            d = 1./std::sqrt(x*x + y*y + DBL_EPSILON);
            dst[6*j + 1] = (float)l2;
            dst[6*j + 4] = (float)(x*d);
            dst[6*j + 5] = (float)(y*d);
        }
    }
}

namespace OMToolkit	{
	OMMatrixCurvature::OMMatrixCurvature(MeshT *mesh)
	{
		m_mesh = mesh;
	}

	bool OMMatrixCurvature::Compute(OpenMesh::VPropHandleT<MatrixT> matrixH, OpenMesh::VPropHandleT<NormalT> curvatureH, OpenMesh::VPropHandleT<AScalarT> curvatureMagH, MatrixType type)
	{
		AScalarT *matrixData = NULL;
		AScalarT *filterDataVert = NULL;
		AScalarT *filterDataHoriz = NULL;
		MatrixT matrix;

		MeshT::VertexIter end = m_mesh->vertices_end();
		for (MeshT::VertexIter vertex = m_mesh->vertices_begin() ; vertex != end; ++vertex)
		{
			
			matrix = m_mesh->property(matrixH, vertex);
			//int size = matrix.size();

			///////////////////////////
			using namespace cv;

			// copy to OpenCV matrix
			Mat cvMatrix(matrix.rows(), matrix.cols(), CV_32FC1);
			for (int i = 0; i < cvMatrix.rows; ++i)
				for (int j = 0; j < cvMatrix.cols; ++j)
					cvMatrix.at<float>(i, j) = matrix(i, j);

			cv::blur(cvMatrix, cvMatrix, cvSize(7,7));
			// Compute partial second derivatives
			Mat Dx, Dy, Dxy;
			Mat kx, ky, kx1, ky1, kx2, ky2;
			getDerivKernels(kx, ky, 2, 0, matrix.rows(), false);
			getDerivKernels(kx1, ky1, 0, 2, matrix.rows(), false);
			getDerivKernels(kx2, ky2, 2, 2, matrix.rows(), false);
			
			sepFilter2D(cvMatrix, Dx, CV_32F, kx, ky);
			sepFilter2D(cvMatrix, Dy, CV_32F, kx1, ky1);
			sepFilter2D(cvMatrix, Dxy, CV_32F, kx2, ky2);
			//filter2D(cvMatrix, Dx, CV_32F, kx);
			//filter2D(cvMatrix, Dy, CV_32F, ky);
			//Sobel( cvMatrix, Dx, CV_32F, 2, 0, matrix.rows() );
			//Sobel( cvMatrix, Dy, CV_32F, 0, 2, matrix.cols() );
			
			// Compute covariance matrix (Autocorrelation)
			Size cvsize = cvMatrix.size();
			Mat cov( cvsize, CV_32FC3 );
			int i, j;

			for( i = 0; i < cvsize.height; i++ )
			{
				float* cov_data = (float*)(cov.data + i*cov.step);
				const float* dxdata = (const float*)(Dx.data + i*Dx.step);
				const float* dydata = (const float*)(Dy.data + i*Dy.step);

				for( j = 0; j < cvsize.width; j++ )
				{
					float dx = dxdata[j];
					float dy = dydata[j];

					cov_data[j*3] = dx*dx;
					cov_data[j*3+1] = dx*dy;
					cov_data[j*3+2] = dy*dy;
				}
			}

			//// filter with box of size 3
			//boxFilter(cov, cov, cov.depth(), Size(3,3), Point(-1,-1), true );
			//////////////////////////////////////////////////////////////////////
			MatrixT hessian(2,2);
			int index = cvMatrix.rows/2;
			float dx = Dx.at<float>(index, index);
			float dy = Dy.at<float>(index, index);
			float dxy = Dxy.at<float>(index, index);
			hessian(0, 0) = dx;
			hessian(0, 1) = dxy;
			hessian(1, 0) = dxy;
			hessian(1, 1) = dy;

			//Mat eigenv(1,1, CV_MAKETYPE(CV_32F, 6));
			//calcEigenValsVecs( cov, eigenv );
			Eigen::SelfAdjointEigenSolver<Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Eigen::AutoAlign>> es;
			es.compute(hessian, true);
			//Eigen::EigenSolver<MatrixT> es(hessian);
			
			//std::cout <<  es.eigenvalues()(0, 0).real() << " " <<  es.eigenvalues()(1, 0).real() << std::endl;
			//////////////////////////////////////////////////////////////////////
			// compute eigen values and eigen vectors
			Mat eigenv(matrix.rows(), matrix.cols(), CV_MAKETYPE(CV_32F, 6));
			calcEigenValsVecs( cov, eigenv );

			// save the result
			NormalT vectorMin(0.0, 0.0, 0.0);
			NormalT vectorMax(0.0, 0.0, 0.0);
			float* eigen = (float*)(eigenv.data + eigenv.step*(matrix.rows()/2));

			j = (matrix.cols()/2);
			OMToolkit::OMTransformationSolver<NormalT> solver(2.0, 7.0, m_mesh->normal(vertex), MeshT::Normal(0.0, 0.0, 0.0), m_mesh->point(vertex));
			//std::cout <<  eigen[6*j + 1] << " " <<  eigen[6*j] << std::endl;

			//std::cout << es.eigenvectors() << std::endl;
			float minEV, maxEV;
			//Eigen::Matrix<float, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor | Eigen::AutoAlign> ev = es.eigenvectors();
			//Eigen::EigenSolver<MatrixT>::EigenvectorType minEvector, maxEvector;
			if ((es.eigenvalues()(0)) < (es.eigenvalues()(1)))
			{
				minEV = es.eigenvalues()(0);
				maxEV = es.eigenvalues()(1);
				vectorMin[0] = es.eigenvectors()(0, 0);
				vectorMin[1] = es.eigenvectors()(0, 1);
				vectorMax[0] = es.eigenvectors()(1, 0);
				vectorMax[1] = es.eigenvectors()(1, 1);
			}
			else
			{
				maxEV = es.eigenvalues()(0);
				minEV = es.eigenvalues()(1);
				vectorMin[0] = es.eigenvectors()(1, 0);
				vectorMin[1] = es.eigenvectors()(1, 1);
				vectorMax[0] = es.eigenvectors()(0, 0);
				vectorMax[1] = es.eigenvectors()(0, 1);
			}
			switch (type)
			{
				case MIN:
					//vector[0] = eigen[6*j + 4];
					//vector[1] = eigen[6*j + 5];
					m_mesh->property(curvatureMagH, vertex) = minEV;
						
					vectorMin = solver.transformToMeshLinear(vectorMin);
					vectorMin.normalize_cond();
					m_mesh->property(curvatureH, vertex) = vectorMin;
					break;
				case MAX:
					m_mesh->property(curvatureMagH, vertex) = maxEV;
					
					
					
					//eigen[6*j];	
																//sqrt(Dx.at<float>(matrix.cols()/2, matrix.cols()/2)*
																//Dx.at<float>(matrix.cols()/2, matrix.cols()/2) + 
																//Dy.at<float>(matrix.cols()/2, matrix.cols()/2)*
																//Dy.at<float>(matrix.cols()/2, matrix.cols()/2));
						
						
						
						
						

					vectorMax = solver.transformToMeshLinear(vectorMax);
					vectorMax.normalize_cond();
					m_mesh->property(curvatureH, vertex) = vectorMax;
					break;
				case MEAN:
					m_mesh->property(curvatureMagH, vertex) = (minEV+maxEV)/2;
					//m_mesh->property(curvatureH, vertex) = vector;
					break;
				case GAUSS:
					m_mesh->property(curvatureMagH, vertex) = minEV*maxEV;
					//m_mesh->property(curvatureH, vertex) = vector;
					break;
			}
			
			
			// transform result vector into curvature
		}
		return true;
	}
} // namespace