// 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 = 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);

}