//==============================================================================
/*! \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/>.
 * 
 * Module description:
 * - Module rasterizes vertex neighbourhood on a tangent raster
 * - Result is sent to connected MDSTk channel
 */

#include "DetectEdges.h"

#include <OMToolkit\IO\OMIO.h>
#include <OMToolkit\OMProjector.h>
#include <OMToolkit\OMTypes.h>
#include <OpenMesh\Tools\Utils\Timer.hh>

///////////////////////////////////////////////////////////////////////////////////////////////////
// Module constants
///////////////////////////////////////////////////////////////////////////////////////////////////

// Module description
const std::string MODULE_DESCRIPTION    = "Module that rasterizes vertex neighbourhood on a tangent raster";

// Additional command line arguments
const std::string MODULE_ARGUMENTS      = "size:relative:resolution:xdir:zdir";

// Additional arguments
const std::string MODULE_ARG_SIZE		= "size";
const std::string MODULE_ARG_RES		= "resolution";
const std::string MODULE_ARG_XDIR		= "xdir";
const std::string MODULE_ARG_ZDIR		= "zdir";
const std::string MODULE_ARG_RELATIVE	= "relative";

const double SIZE_DEFAULT				= 2.0;
const double RES_DEFAULT				= 3.0;
const bool RELATIVE_DEFAULT				= false;

const std::string ZDIR_CURVATURE		= "curvature";
const std::string ZDIR_ZDIR				= "z";
const std::string ZDIR_NORMALS			= "normals";
const std::string ZDIR_DEFAULT			= ZDIR_ZDIR;

const std::string XDIR_CURVATURE		= "curvature";
const std::string XDIR_NONE				= "none";
const std::string XDIR_DEFAULT			= XDIR_NONE;

// Type of accepted mesh
typedef OMToolkit::Types::ModuleMeshd	MeshT;

///////////////////////////////////////////////////////////////////////////////////////////////////
// Constructor
///////////////////////////////////////////////////////////////////////////////////////////////////
OMComputeMatrices::OMComputeMatrices(const std::string& sDescription) : mds::mod::CModule(sDescription)
{
    allowArguments(MODULE_ARGUMENTS);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Destructor
///////////////////////////////////////////////////////////////////////////////////////////////////
OMComputeMatrices::~OMComputeMatrices()
{
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Do on startup
///////////////////////////////////////////////////////////////////////////////////////////////////
bool OMComputeMatrices::startup()
{
	// Disable all OpenMesh errorlogs (for not mix MDSTk log)
	omlog().disable();
	omerr().disable();
	omout().disable();
    
	// Note
    MDS_LOG_NOTE("Module startup");

    // Test of existence of input and output channel
    if( getNumOfInputs() != 1 || getNumOfOutputs() != 1 )
    {
        MDS_CERR('<' << m_sFilename << "> Wrong number of input and output channels" << std::endl);
        return false;
    }

	m_size = SIZE_DEFAULT;
	m_Arguments.value(MODULE_ARG_SIZE, m_size);

	m_resolution = RES_DEFAULT;
	m_Arguments.value(MODULE_ARG_RES, m_resolution);

	m_direction = XDIR_DEFAULT;
	m_Arguments.value(MODULE_ARG_XDIR, m_direction);

	m_relative = RELATIVE_DEFAULT;
	if (m_Arguments.exists(MODULE_ARG_RELATIVE)) m_relative = true;

	if (m_size <= 0.0 || m_resolution <= 0.0 || ((int)m_resolution)%2 == 0)
	{
		MDS_CERR('<' << m_sFilename << "> Wrong size or resolution of a tangent matrix" << std::endl);
		return false;
	}

	m_directionZ = ZDIR_ZDIR;
	m_Arguments.value(MODULE_ARG_ZDIR, m_directionZ);
	
    // O.K.
    return true;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Main module loop
///////////////////////////////////////////////////////////////////////////////////////////////////
bool OMComputeMatrices::main()
{
    //// Note
    MDS_LOG_NOTE("Module main function");

    //// I/O channels
    mds::mod::CChannel *pIChannel = getInput(0);
    mds::mod::CChannel *pOChannel = getOutput(0);

	 // Is any input?
    if( !pIChannel->isConnected() )
    {
        return false;
    }

    // Wait for data
    if( pIChannel->wait(1000) )
    {
		// Mesh specification and read options
		MeshT mesh;
		OMToolkit::IO::Options opt = OMToolkit::IO::Options::Default;
	
		// Read and save mesh
		if (OMToolkit::IO::readMesh(mesh, *pIChannel, opt))
		{

			////////////////////////////////////////////////////////////////////////////////////////
			// filter kernel
			std::ifstream file;
			file.open("sobel9x9.flt");

			std::vector<MeshT::VertexMatrix> m_filterKernel;
			m_filterKernel.clear();

			// while not eof, we read
			while (!file.eof())
			{
				std::string header;
				file >> header;

				// check header
				if (header != "Matrix") return false;
				int rows, cols;
				file >> rows;
				file >> cols;

				// check rows and cols information
				if (rows <= 0 || cols <= 0) return false;

				m_filterKernel.push_back(MeshT::VertexMatrix(rows, cols));

				// read matrix
				for (int r = 0; r < rows; ++r)
					for (int c = 0; c < rows; ++c)
					{
						if (file.eof()) return false;
						file >> m_filterKernel.back()(r, c);
					}
			}
			file.close();
			////////////////////////////////////////////////////////////////////////////////////////
			// create grad

			
			mesh.request_face_normals();
			mesh.request_vertex_normals();
			mesh.update_normals();
			double biggest = 0;

			for (MeshT::VertexIter vertex = mesh.vertices_begin(); vertex != mesh.vertices_end(); ++vertex)
			{
				MeshT::Normal vector(0.0, 0.0, 0.0);
				OMToolkit::OMTransformationSolver<typename MeshT::Normal> solver(1.0, 9.0, mesh.normal(vertex), MeshT::Normal(0.0, 0.0, 0.0), mesh.point(vertex));
				
				// load matrix data
				MeshT::AttributeScalar *first = mesh.property(mesh.getMatrixHandle(), vertex).data();
				MeshT::AttributeScalar *filter = NULL;
				MeshT::AttributeScalar result = 0.0;

				for (unsigned int i = 0; i < 2; ++i)
				{
					MeshT::AttributeScalar aux = 0.0;
					filter = m_filterKernel[i].data();
					int i_max = m_filterKernel[i].size();
					for (int j = 0; j < i_max; ++j) 
							aux += filter[j] * first[j];
		
					vector[i] = aux;
				}
				//float factor = mesh.property(mesh.getVertexAttributeHandle(), vertex)[0];
				//vector = vector % MeshT::Normal(0.0, 0.0, 1.0);
				/*MDS_LOG_NOTE("");
				MDS_LOG_NOTE(vector[0] << " " << vector[1] << " " << vector[2]);*/
				vector = solver.transformToMeshLinear(vector);
				//MDS_LOG_NOTE(vector[0] << " " << vector[1] << " " << vector[2]);
				if (vector.norm() > 0)
					vector.normalize();
				//vector *= factor;
				
				mesh.property(mesh.getCurvatureHandle(), vertex) = vector;

			}

			//std::ofstream xxx;
			//xxx.open("out.txt");
			//for (MeshT::VertexIter vertex = mesh.vertices_begin(); vertex != mesh.vertices_end(); ++vertex)
			//{
			//	MeshT::VertexMatrix matrix = mesh.getMatrix(vertex);
			//	for (unsigned int i = 0; i < 9; i++)
			//	{
			//		for (unsigned int j = 0; j < 9; j++)
			//		{
			//			xxx << std::setw(8) << std::setprecision(5) << matrix(i, j) << " ";
			//		}

			//		xxx << std::endl;
			//	}
			//	xxx << std::endl;
			//}
			//xxx.close();

			
			////////////////////////////////////////////////////////////////////////////////////////
			// Supress non maximum vertices
			std::vector<double> values;
			for (MeshT::VertexIter vertex = mesh.vertices_begin(); vertex != mesh.vertices_end(); ++vertex)
			{

				MeshT::Normal gradient = mesh.curvature(vertex);
				/*for (MeshT::VVIter varound = mesh.vv_begin(vertex); varound; ++varound)
				{
					gradient += mesh.curvature(varound);
				}
				if (gradient.norm() != 0)
					gradient.normalize();*/
				MeshT::HalfedgeHandle greatest;
				MeshT::HalfedgeHandle lowest;
				double Langle = 99.0;
				double gAngle = 0.0;
				MeshT::Normal vec;
				bool ok = !mesh.is_boundary(vertex) && gradient.norm() > 0;
				for (MeshT::VOHIter edge = mesh.voh_begin(vertex); edge; ++edge)
				{
					if (ok)
					{
						mesh.calc_edge_vector(edge, vec);
						vec.normalize();
						double angle = acos(vec | gradient);
					
						if (angle < Langle)
						{
							Langle = angle;
							lowest = edge;
						}
					
						if (angle > gAngle)
						{
							gAngle = angle;
							greatest = edge;
						}
					}
					//else 
					//std::cout << mesh.point(vertex)[0] << " ";
					//std::cout << mesh.point(vertex)[1] << " ";
					//std::cout << mesh.point(vertex)[2] << std::endl;

					//std::cout << mesh.point(mesh.from_vertex_handle(edge))[0] << " ";
					//std::cout << mesh.point(mesh.from_vertex_handle(edge))[1] << " ";
					//std::cout << mesh.point(mesh.from_vertex_handle(edge))[2] << std::endl;
					//std::cout << acos(vec | gradient) << std::endl;
					//std::cout << vec[0] << " " << vec[1] << " " << vec[2] << std::endl;
					//std::cout << gradient[0] << " " << gradient[1] << " " << gradient[2] << std::endl;
					//std::cout << edge.handle().idx() << std::endl;
				}
				
				//if (gradient.norm() > 0)
				//std::cout << gradient[0] << " " << gradient[1] << " " << gradient[2] << std::endl;
				//std::cout << gradient.norm() << " " << lowest << " " << greatest << std::endl;
				//std::cout << Langle << " " << gAngle << " " << ok << std::endl;
				values.push_back(mesh.getAttributes(vertex)[0]);
				if (ok && (mesh.getAttributes(mesh.to_vertex_handle(lowest))[0] <= mesh.getAttributes(vertex)[0] &&
					mesh.getAttributes(mesh.to_vertex_handle(greatest))[0] <= mesh.getAttributes(vertex)[0]))
				{
					mesh.property(mesh.getFlagHandle(), vertex) = 1;
					//values.push_back(mesh.getAttributes(vertex)[0]);
				}
				else mesh.property(mesh.getFlagHandle(), vertex) = 0;
				
				//std::cout << "===================="<< std::endl;

				
			}

			std::sort(values.begin(), values.end());
			double Tsmall	= values[values.size()/10];
			double Tbig		= values[values.size()/1.4];
			for (MeshT::VertexIter vertex = mesh.vertices_begin(); vertex != mesh.vertices_end(); ++vertex)
			{
				if (mesh.property(mesh.getFlagHandle(), vertex) == 1 && mesh.getAttributes(vertex)[0] > Tbig)
				{
					mesh.set_color(vertex, MeshT::Color(200.0, 200.0, 230.0, 255.0));
				}
				else if (mesh.property(mesh.getFlagHandle(), vertex) == 1 && mesh.getAttributes(vertex)[0] > Tsmall)
				{
					mesh.set_color(vertex, MeshT::Color(0.0, 255.0, 0.0, 255.0));
				}
				else mesh.set_color(vertex, MeshT::Color(0.0, 0.0, 255.0, 255.0));
			}

			
			////////////////////////////////////////////////////////////////////////////////////////
			
			// write output
			if (!OMToolkit::IO::writeMesh(mesh, *pOChannel))
			{
				MDS_CERR('<' << m_sFilename << "> Failed to write output data" << std::endl);
				return false;
			}
		}
		// Error on input
		else 
		{
			MDS_CERR('<' << m_sFilename << "> Failed to read input mesh data" << std::endl);
			return false;
		}

		return false;
	}
    else
    {
        MDS_LOG_NOTE("Wait timeout");
    }

    // Returning 'true' means to continue processing the input channel
    return true;	
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// On module shutdown
///////////////////////////////////////////////////////////////////////////////////////////////////
void OMComputeMatrices::shutdown()
{
    // Note
    MDS_LOG_NOTE("Module shutdown");
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Writes extended use of this module
///////////////////////////////////////////////////////////////////////////////////////////////////
void OMComputeMatrices::writeExtendedUsage(std::ostream& Stream)
{
    MDS_CERR("Necessary arguments: [-size matrixSize] [-relative] [-resolution matrixResolution] [-xdir XAxisDir]" << std::endl);
    MDS_CERR("Options:" << std::endl);
	MDS_CERR("  -size Specifies matrix size in mesh space." << std::endl);
	MDS_CERR("	  -Argument is double precision number greater than 0.0" << std::endl);
	MDS_CERR("  -relative Use this argument if you want to specify matrix size relatively." << std::endl);
	MDS_CERR("	  -If set, length is not static but is computed as matrixSize * medianOfEdgeLengths" << std::endl);
	MDS_CERR("  -resolution Specifies square matrix dimension in one direction." << std::endl);
	MDS_CERR("	  -Argument is double precision number greater than 0.0." << std::endl);
	MDS_CERR("	  -Must be odd number (for filtration purposes)" << std::endl);
	MDS_CERR("  -xdir Specifies vector, which will be used to align matrix X direction." << std::endl);
	MDS_CERR("	  " << XDIR_CURVATURE << " - Option sets X direction as computed curvature vector." << std::endl);
	MDS_CERR("	  " << XDIR_NONE      << " - Option sets X direction randomly (does not align)." << std::endl);
    MDS_CERR(std::endl);
}

///////////////////////////////////////////////////////////////////////////////////////////////////
// Main - executing a module
///////////////////////////////////////////////////////////////////////////////////////////////////
int main(int argc, char *argv[])
{
    // Creation of a module using smart pointer
    OMComputeMatricesPtr spModule(new OMComputeMatrices(MODULE_DESCRIPTION));

    // Initialize and execute the module
    if( spModule->init(argc, argv) )
    {
        spModule->run();
    }

    // Console application finished
    return 0;
}

