#include "drake/geometry/proximity/obj_to_surface_mesh.h"

#include <filesystem>
#include <fstream>
#include <istream>
#include <memory>
#include <numeric>
#include <optional>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>

#include <fmt/format.h>
#include <tiny_obj_loader.h>

#include "drake/common/drake_assert.h"
#include "drake/common/text_logging.h"
#include "drake/geometry/proximity/triangle_surface_mesh.h"

namespace drake {
namespace geometry {
namespace {

using drake::internal::DiagnosticDetail;
using drake::internal::DiagnosticPolicy;
using Eigen::Vector3d;

// TODO(DamrongGuoy): Refactor the tinyobj usage between here and
//  ProximityEngine.

/*
 Converts vertices of tinyobj to vertices of TriangleSurfaceMesh.
 @param tinyobj_vertices
     Vertices from tinyobj represented as `std::vector` of floating-point
     numbers.
 @param scale
     A scale to coordinates.
 @return
     Vertices for TriangleSurfaceMesh.
 @pre
     The size of `tinyobj_vertices` is divisible by three.
 */
std::vector<Vector3d> TinyObjToSurfaceVertices(
    const std::vector<tinyobj::real_t>& tinyobj_vertices, const double scale) {
  // Vertices from tinyobj are in a vector of floating-point numbers like this:
  //     tinyobj_vertices = {c₀,c₁,c₂, c₃,c₄,c₅, c₆,c₇,c₈,...}
  //                      = {x, y, z,  x, y, z,  x, y, z,...}
  // We will convert to a vector of Vector3<double> like this:
  //     vertices = {{c₀,c₁,c₂}, {c₃,c₄,c₅}, {c₆,c₇,c₈},...}
  //              = {    v0,         v1,         v2,...}
  const int num_coords = tinyobj_vertices.size();
  DRAKE_DEMAND(num_coords % 3 == 0);
  std::vector<Vector3d> vertices;
  vertices.reserve(num_coords / 3);

  auto iter = tinyobj_vertices.begin();
  while (iter != tinyobj_vertices.end()) {
    double x = *(iter++) * scale;
    double y = *(iter++) * scale;
    double z = *(iter++) * scale;
    vertices.emplace_back(Vector3<double>(x, y, z));
  }
  return vertices;
}

/*
 Converts faces of tinyobj::mesh_t to faces of TriangleSurfaceMesh.
 @param[in] mesh
     The mesh from tinyobj.
 @param[out] faces
     Triangles from previous meshes plus new triangles from this `mesh` on
     return.
 @pre
     Every face is a triangle.
 */
void TinyObjToSurfaceFaces(const tinyobj::mesh_t& mesh,
                           std::vector<SurfaceTriangle>* faces) {
  //
  // In general, tinyobj::mesh_t::num_face_vertices is a list of number
  // of vertices of each polygonal face like this:
  //     mesh.num_face_vertices = {n₀, n₁, n₂,...},
  // where faceᵢ has nᵢ vertices. In this function, we require every face to
  // be a triangle, so every nᵢ is 3 like this:
  //     mesh.num_face_vertices = {3, 3, 3,...}.
  //
  // In general, tinyobj::mesh_t::indices concatenates tinyobj::index_t of
  // vertices of each face like this:
  //     mesh.indices = {v0₀,v0₁,...,v0ₙ₀₋₁, v1₀,v1₁,...,v1ₙ₁₋₁, ...}.
  //                     -------face0------  -------face1------
  // Because this function requires the faces to be triangles, it must look
  // like this:
  //     mesh.indices = {v0₀,v0₁,v0₂, v1₀,v1₁,v1₂, ...}.
  //                     ---face0---  ---face1---
  //
  // Each tinyobj::index_t consists of vertex_index, normal_index, and
  // texcoord_index. In this function, we use only vertex_index.
  //
  const int num_faces = mesh.num_face_vertices.size();
  const int num_indices = mesh.indices.size();
  // Although we will validate each face as a triangle individually, we make
  // sure that there are enough vertices for that to be true as an initial
  // check.
  DRAKE_DEMAND(3 * num_faces == num_indices);
  for (int face = 0; face < num_faces; ++face) {
    DRAKE_DEMAND(mesh.num_face_vertices[face] == 3);
    const int vertex_indices[3] = {mesh.indices[3 * face].vertex_index,
                                   mesh.indices[3 * face + 1].vertex_index,
                                   mesh.indices[3 * face + 2].vertex_index};
    faces->emplace_back(vertex_indices);
  }
}

}  // namespace

namespace internal {
std::optional<TriangleSurfaceMesh<double>> DoReadObjToSurfaceMesh(
    std::istream* input_stream, const double scale,
    const std::optional<std::string>& mtl_basedir,
    const ObjParseConfig& config) {
  tinyobj::attrib_t attrib;                    // Used for vertices.
  std::vector<tinyobj::shape_t> shapes;        // Used for triangles.
  std::vector<tinyobj::material_t> materials;  // Not used.
  std::string warn;
  std::string err;
  std::unique_ptr<tinyobj::MaterialReader> readMatFn;
  if (mtl_basedir) {
    readMatFn = std::make_unique<tinyobj::MaterialFileReader>(*mtl_basedir);
  }
  // triangulate non-triangle faces.
  bool triangulate = true;

  bool ret = tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err,
                              input_stream, readMatFn.get(), triangulate);
  if (!ret || !err.empty()) {
    const std::string err_message =
        fmt::format("Error parsing Wavefront obj data : {}", err);
    config.diagnostic.Error(err_message);
    return std::nullopt;
  }
  if (!warn.empty()) {
    warn = "Warning parsing Wavefront obj data : " + warn;
    if (warn.back() == '\n') {
      warn.pop_back();
    }
    config.diagnostic.Warning(warn);
  }
  if (shapes.size() == 0) {
    const char* err_message = "The Wavefront obj data has no faces.";
    config.diagnostic.Error(err_message);
    return std::nullopt;
  }

  if (config.allowed_shape_count > 0 &&
      static_cast<int>(shapes.size()) > config.allowed_shape_count) {
    const std::string err_message = fmt::format(
        "The Wavefront obj data defines {} unique shapes; only {} allowed.",
        shapes.size(), config.allowed_shape_count);
    config.diagnostic.Error(err_message);
    return std::nullopt;
  }

  std::vector<Vector3d> vertices =
      TinyObjToSurfaceVertices(attrib.vertices, scale);

  // tinyobj stores vertices from all objects in attrib.vertices but stores
  // faces from each object separately. We will keep all faces together in
  // the return TriangleSurfaceMesh. First we calculate the total number of
  // faces of all objects, so we can pre-allocate memory for all faces of
  // TriangleSurfaceMesh.
  int total_num_faces =
      std::accumulate(shapes.begin(), shapes.end(), 0,
                      [](int sum, const tinyobj::shape_t& shape) {
                        return sum + shape.mesh.num_face_vertices.size();
                      });
  std::vector<SurfaceTriangle> faces;
  faces.reserve(total_num_faces);
  for (const tinyobj::shape_t& shape : shapes) {
    TinyObjToSurfaceFaces(shape.mesh, &faces);
  }

  return TriangleSurfaceMesh<double>(std::move(faces), std::move(vertices));
}

}  // namespace internal

TriangleSurfaceMesh<double> ReadObjToTriangleSurfaceMesh(
    const std::string& filename, const double scale,
    std::function<void(std::string_view)> on_warning) {
  std::ifstream input_stream(filename);
  if (!input_stream.is_open()) {
    throw std::runtime_error("Cannot open file '" + filename + "'");
  }
  const std::string mtl_basedir =
      std::filesystem::path(filename).parent_path().string() + "/";

  DiagnosticPolicy policy;
  if (on_warning != nullptr) {
    policy.SetActionForWarnings([&on_warning](const DiagnosticDetail& detail) {
      on_warning(detail.FormatWarning());
    });
  }

  // We will either throw or return a mesh here.
  return *internal::DoReadObjToSurfaceMesh(&input_stream, scale, mtl_basedir,
                                           {.diagnostic = policy});
}

TriangleSurfaceMesh<double> ReadObjToTriangleSurfaceMesh(
    std::istream* input_stream, const double scale,
    std::function<void(std::string_view)> on_warning) {
  DRAKE_THROW_UNLESS(input_stream != nullptr);

  DiagnosticPolicy policy;
  if (on_warning != nullptr) {
    policy.SetActionForWarnings([&on_warning](const DiagnosticDetail& detail) {
      on_warning(detail.FormatWarning());
    });
  }

  // We will either throw or return a mesh here.
  return *internal::DoReadObjToSurfaceMesh(input_stream, scale,
                                           std::nullopt /* mtl_basedir */,
                                           {.diagnostic = policy});
}

}  // namespace geometry
}  // namespace drake
