/* The header file. */

// Given a set of points, defined by corresponding entries in x and y, compute the triangulation
// for the points.
// Returns a list of indices where each set of triples are the indices of a triangle.
// You could change this to double instead of float if you like. All the important stuff in the inside is templated.
void Triangulator(const std::vector<float>& x, const std::vector<float>& y, std::vector<int>* indices);


/* The CC file. */
#include <vector>

namespace {

template <typename T>
struct Vector2 {
  using Type = T;

  Vector2(const Vector2<T> &v) = default;
  Vector2(Vector2<T>&&) = default;

  Vector2(T vx, T vy)
    : x(vx), y(vy) {}

  T dist2(const Vector2<T> &v) const {
    const T dx = x - v.x;
    const T dy = y - v.y;
    return dx * dx + dy * dy;
  }

  T norm2() const { return x * x + y * y; }

  T x, y;
};

struct IndexTriangle {
  IndexTriangle(int a, int b, int c)
    : a(a), b(b), c(c), bad(false) {}
  int a, b, c;
  bool bad;
};

struct IndexEdge {
  IndexEdge(int a, int b)
    : a(a), b(b), bad(false) {}

  bool IsSame(const IndexEdge& e) {
    return (e.a == a && e.b == b) || (e.a == b && e.b == a);
  }

  int a, b;
  bool bad;
};

template<typename VertexType>
std::vector<IndexTriangle> Triangulate(std::vector<VertexType> &vertices) {
  using T = typename VertexType::Type;
  // Determine the super triangle.
  auto CompareX = [](const VertexType& a, const VertexType& b) { return a.x < b.x; };
  auto CompareY = [](const VertexType& a, const VertexType& b) { return a.y < b.y; };
  T minX = std::min_element(vertices.begin(), vertices.end(), CompareX)->x;
  T minY = std::min_element(vertices.begin(), vertices.end(), CompareY)->y;
  T maxX = std::max_element(vertices.begin(), vertices.end(), CompareX)->x;
  T maxY = std::max_element(vertices.begin(), vertices.end(), CompareY)->y;

  const T dx = maxX - minX;
  const T dy = maxY - minY;
  const T deltaMax = std::max(dx, dy);
  const T midx = (minX + maxX) / 2;
  const T midy = (minY + maxY) / 2;

  const VertexType p1(midx - 20 * deltaMax, midy - deltaMax);
  const VertexType p2(midx, midy + 20 * deltaMax);
  const VertexType p3(midx + 20 * deltaMax, midy - deltaMax);

  // Create a list of triangles, and add the super triangle in it.
  std::vector<IndexTriangle> triangles = {IndexTriangle(-1, -2, -3)};

  const auto& Lookup = [&](int index) {
    if (index >= 0) {
      // The coordinates do not belong to the super triangle. This is the common case.
      return vertices[index];
    } else if (index == -1) {
      return p1;
    } else if (index == -2) {
      return p2;
    } else {
      return p3;
    }
  };

  auto CircleContains = [&](const IndexTriangle& t, const VertexType &v) {
    const VertexType& A = Lookup(t.a);
    const VertexType& B = Lookup(t.b);
    const VertexType& C = Lookup(t.c);
    const T Asq = A.norm2();
    const T Bsq = B.norm2();
    const T Csq = C.norm2();
    // https://en.wikipedia.org/wiki/Circumscribed_circle#Circumcenter_coordinates
    const T circum_x = (Asq * (C.y - B.y) + Bsq * (A.y - C.y) + Csq * (B.y - A.y)) /
                   2 / (A.x * (C.y - B.y) + B.x * (A.y - C.y) + C.x * (B.y - A.y));
    const T circum_y = (Asq * (C.x - B.x) + Bsq * (A.x - C.x) + Csq * (B.x - A.x)) /
                   2 / (A.y * (C.x - B.x) + B.y * (A.x - C.x) + C.y * (B.x - A.x));

    const VertexType circum(circum_x, circum_y);
    // Test if v is closer to the center of the circle than A (which is on the radius).
    return v.dist2(circum) <= A.dist2(circum);
  };

  for (int vertex_index = 0; vertex_index < vertices.size(); ++vertex_index) {
    std::vector<IndexEdge> polygon;
    for (IndexTriangle& t : triangles) {
      if (CircleContains(t, vertices[vertex_index])) {
        t.bad = true;
        polygon.emplace_back(t.a, t.b);
        polygon.emplace_back(t.b, t.c);
        polygon.emplace_back(t.c, t.a);
      }
    }

    triangles.erase(std::remove_if(triangles.begin(), triangles.end(),
                                   [](const IndexTriangle &t){ return t.bad; }), triangles.end());
    // Remove duplicate edges.
    for (auto e1 = polygon.begin(); e1 != polygon.end(); ++e1) {
      for (auto e2 = e1 + 1; e2 != polygon.end(); ++e2) {
        if(e1->IsSame(*e2)) {
          e1->bad = true;
          e2->bad = true;
        }
      }
    }
    polygon.erase(std::remove_if(
        polygon.begin(), polygon.end(), [](const IndexEdge &e) { return e.bad; }), polygon.end());

    for (const IndexEdge& edge : polygon) {
      triangles.emplace_back(edge.a, edge.b, vertex_index);
    }
  }
  // Remove triangles that include the super triangle.
  auto IncludesSuperTriangle = [](const IndexTriangle &t) { return t.a < 0 || t.b < 0 || t.c < 0; };
  triangles.erase(std::remove_if(
      triangles.begin(), triangles.end(), IncludesSuperTriangle), triangles.end());

  return triangles;
}

}  // anonymous namespace

void Triangulator(const std::vector<float>& x,
                  const std::vector<float>& y,
                  std::vector<int>* indices) {
  std::vector<Vector2<float>> points;
  points.reserve(x.size());
  for (int i = 0; i < x.size(); ++i) {
    points.emplace_back(x[i], y[i]);
  }

  std::vector<IndexTriangle> triangles = Triangulate(points);
  indices->reserve(triangles.size() * 3);
  // Vertices of triangle:
  //   points[tri.a] (or x[tri.a], y[tri.b])
  //   points[tri.a]
  //   points[tri.b]
  // Edges of triangle:
  //   points[tri.a] -- points[tri.b]
  //   points[tri.a] -- points[tri.c]
  //   points[tri.b] -- points[tri.c]
  // Indices VBO are stored in indices.
  // http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-9-vbo-indexing/
  for (const auto& tri : triangles) {
    indices->push_back(tri.a);
    indices->push_back(tri.b);
    indices->push_back(tri.c);
  }
}