// TnzCore includes
#include "tmeshimage.h"
 
// tcg includes
#include "tcg/tcg_misc.h"
#include "tcg/tcg_mesh_bgl.h"
 
using namespace tcg::bgl;
 
// Boost includes
#include <boost/graph/properties.hpp>
#include <boost/graph/breadth_first_search.hpp>
 
// STD includes
#include <queue>
 
#include "ext/plastichandle.h"
 
#include <cstring>
 
//***********************************************************************************************
//    Distance building
//***********************************************************************************************
 
namespace
{
 
typedef tcg::Mesh<TTextureVertex, tcg::Edge, tcg::FaceN<3>> Graph;
 
//----------------------------------------------------------------------------------
 
struct DistanceGreater {
	float *m_distances;
 
public:
	DistanceGreater(float *distances) : m_distances(distances) {}
 
	bool operator()(int a, int b) { return m_distances[a] > m_distances[b]; }
};
 
} // namespace
 
//==================================================================================
 
namespace local
{
 
struct BFS_DistanceBuilder {
	typedef boost::graph_traits<Graph>::edge_descriptor edge_descr;
 
	float *m_distances; //!< (NOT owned) Distances from selected vertex
	UCHAR *m_colormap;  //!< (NOT owned) Graph BFS colormap
 
public:
	BFS_DistanceBuilder(float *distances, UCHAR *colormap)
		: m_distances(distances), m_colormap(colormap) {}
 
	void tree_edge(const edge_descr &e, const Graph &g)
	{
		int v, v0 = -1, v1 = tcg::bgl::target(e, g);
		double d, dMin = (std::numeric_limits<double>::max)();
 
		assert(m_colormap[v1] == boost::white_color);
 
		// Search for the distance-nearest found vertex
		const Graph::vertex_type &vx1 = g.vertex(v1);
 
		Graph::vertex_type::edges_const_iterator et, eEnd(vx1.edgesEnd());
		for (et = vx1.edgesBegin(); et != eEnd; ++et) {
			v = g.edge(*et).otherVertex(v1);
			if (m_colormap[v] != boost::white_color) {
				d = tcg::point_ops::dist(g.vertex(v).P(), vx1.P());
				if (d < dMin)
					v0 = v, dMin = d;
			}
		}
 
		assert(v0 >= 0);
 
		// Just add to the distance from that vertex
		m_distances[v1] = m_distances[v0] + dMin;
	}
 
	// Unused bfs methods
 
	void initialize_vertex(int v, const Graph &g) {}
	void discover_vertex(int v, const Graph &g) {}
	void examine_vertex(int v, const Graph &g) {}
	void finish_vertex(int v, const Graph &g) {}
 
	void examine_edge(const edge_descr &e, const Graph &g) {}
	void non_tree_edge(const edge_descr &e, const Graph &g) {}
	void gray_target(const edge_descr &e, const Graph &g) {}
	void black_target(const edge_descr &e, const Graph &g) {}
};
 
} // namespace local
 
//==================================================================================
 
bool buildDistances(float *distances, const TTextureMesh &mesh, const TPointD &pos,
					int *faceHint)
{
	using namespace local;
 
	// First off, find the face containing the specified vertex
	int localF = -1, &f = faceHint ? *faceHint : localF;
 
	if (f < 0 || f >= mesh.facesCount() || !mesh.faceContains(f, pos))
		f = mesh.faceContaining(pos);
 
	if (f < 0)
		return false;
 
	int vCount = mesh.verticesCount();
 
	// Prepare the interpolation builder
	UCHAR *colorMap = (UCHAR *)calloc(vCount, sizeof(UCHAR));
	BFS_DistanceBuilder visitor(distances, colorMap);
 
	DistanceGreater gr(distances);
	std::priority_queue<int, std::vector<int>, DistanceGreater> verticesQueue(gr);
 
	int v0, v1, v2;
	mesh.faceVertices(f, v0, v1, v2);
 
	// Prepare BFS data
	distances[v0] = distances[v1] = distances[v2] = 0.0f;
 
	verticesQueue.push(v0), colorMap[v0] = boost::gray_color;
	verticesQueue.push(v1), colorMap[v1] = boost::gray_color;
 
	// Launch BFS algorithm
	boost::breadth_first_visit((const Graph &)mesh, v2, verticesQueue, visitor, colorMap);
 
	free(colorMap);
	return true;
}
 
//----------------------------------------------------------------------------------
 
void buildSO(double *so, const TTextureMesh &mesh,
			 const std::vector<PlasticHandle> &handles, int *faceHints)
{
	int v, vCount = mesh.verticesCount();
 
	// Build the interpolant function data
	const TRectD &bbox = mesh.getBBox();
 
	const double len = std::max(bbox.getLx(), bbox.getLy()), val = 1e-8;
	const double k = -log(val) / len;
 
	// Allocate / initialize arrays
	float *distances = (float *)malloc(vCount * sizeof(float));
	double *wSums = (double *)calloc(vCount, sizeof(double));
 
	memset(so, 0, vCount * sizeof(double));
 
	// Iterate handles - for each, add the corresponding interpolant
	int h, hCount = handles.size();
	for (h = 0; h != hCount; ++h) {
		const PlasticHandle &handle = handles[h];
 
		if (!buildDistances(distances, mesh, handle.m_pos, faceHints ? &faceHints[h] : 0))
			continue;
 
		for (v = 0; v != vCount; ++v) {
			double w = fabs(distances[v]);
 
			wSums[v] += w = exp(-k * w) / (1e-3 + w);
			so[v] += w * handle.m_so;
		}
	}
 
	// Normalize so values by the wSums
	for (v = 0; v != vCount; ++v)
		if (wSums[v] != 0.0)
			so[v] /= wSums[v];
 
	// Cleanup
	free(wSums);
	free(distances);
}