// 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=""></boost>

#include <boost breadth_first_search.hpp="" graph=""></boost>

// STD includes

#include <queue></queue>

#include "ext/plastichandle.h"

#include <cstring></cstring>

//***********************************************************************************************

//    Distance building

//***********************************************************************************************

namespace

{

typedef tcg::Mesh<ttexturevertex, tcg::edge,="" tcg::facen<3="">> Graph;</ttexturevertex,>

//----------------------------------------------------------------------------------

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;</graph>

	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)();</double>

		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,>

	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)</plastichandle>

{

	int v, vCount = mesh.verticesCount();

	// Build the interpolant function data

	const TRectD &bbox = mesh.getBBox();

	const double len = tmax(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);

}