libfs.h

Go to the documentation of this file.

#pragma once

#include <iostream>
#include <climits>
#include <stdio.h>
#include <vector>
#include <fstream>
#include <cassert>
#include <sstream>
#include <stdexcept>
#include <map>
#include <unordered_set>
#include <unordered_map>
#include <cmath>
#include <algorithm>
#include <chrono>
#include <cstdint>

#define LIBFS_VERSION "0.4.0"
#define LIBFS_VERISION_MAJOR 0
#define LIBFS_VERISION_MINOR 4
#define LIBFS_VERISION_PATCH 0


// Set apptag (printed as prefix of debug messages) for debug messages.
// Users can overwrite this by defining LIBFS_APPTAG before including 'libfs.h'.
#ifndef LIBFS_APPTAG
#define LIBFS_APPTAG "[libfs] "
#endif

// Set default.
#define LIBFS_DBG_WARNING

// If the user wants something below our default, remove our default.
#ifdef LIBFS_DBG_NONE
#undef LIBFS_DBG_WARNING
#endif

#ifdef LIBFS_DBG_CRITICAL
#undef LIBFS_DBG_WARNING
#endif

#ifdef LIBFS_DBG_ERROR
#undef LIBFS_DBG_WARNING
#endif

// Ensure that the user does not have to define all debug levels
// up to the one they actually want, by defining all lower ones for them.
#ifdef LIBFS_DBG_EXCESSIVE
#define LIBFS_DBG_VERBOSE
#endif

#ifdef LIBFS_DBG_VERBOSE
#define LIBFS_DBG_INFO
#endif

#ifdef LIBFS_DBG_INFO
#define LIBFS_DBG_WARNING
#endif

#ifdef LIBFS_DBG_WARNING
#define LIBFS_DBG_ERROR
#endif

#ifdef LIBFS_DBG_ERROR
#define LIBFS_DBG_CRITICAL
#endif

// End of debug handling.

namespace fs
{

  namespace util
  {

    tm _localtime(const std::time_t &time)
    {
      std::tm tm_snapshot;
#if (defined(WIN32) || defined(_WIN32) || defined(__WIN32__))
      ::localtime_s(&tm_snapshot, &time);
#else
      ::localtime_r(&time, &tm_snapshot); // POSIX
#endif
      return tm_snapshot;
    }

    std::string time_tag(std::chrono::system_clock::time_point t)
    {
      auto as_time_t = std::chrono::system_clock::to_time_t(t);
      struct tm tm;
      char time_buffer[64];
      // if (::gmtime_r(&as_time_t, &tm)) {
      tm = _localtime(as_time_t);
      if (std::strftime(time_buffer, sizeof(time_buffer), "%F %T", &tm))
      {
        return std::string{time_buffer};
      }
      throw std::runtime_error("Failed to get current date as string");
    }

    const std::string LOGTAG_CRITICAL = "CRITICAL";

    const std::string LOGTAG_ERROR = "ERROR";

    const std::string LOGTAG_WARNING = "WARNING";

    const std::string LOGTAG_INFO = "INFO";

    const std::string LOGTAG_VERBOSE = "VERBOSE";

    const std::string LOGTAG_EXCESSIVE = "EXCESSIVE";

    inline void log(std::string const &message, std::string const loglevel = "INFO")
    {
      std::cout << LIBFS_APPTAG << "[" << loglevel << "] [" << fs::util::time_tag(std::chrono::system_clock::now()) << "] " << message << "\n";
    }

    inline bool ends_with(std::string const &value, std::string const &suffix)
    {
      if (suffix.size() > value.size())
        return false;
      return std::equal(suffix.rbegin(), suffix.rend(), value.rbegin());
    }

    inline bool ends_with(std::string const &value, std::initializer_list<std::string> suffixes)
    {
      for (auto suffix : suffixes)
      {
        if (ends_with(value, suffix))
        {
          return true;
        }
      }
      return false;
    }

    template <typename T>
    std::vector<std::vector<T>> v2d(std::vector<T> values, size_t num_cols)
    {
      std::vector<std::vector<T>> result;
      for (std::size_t i = 0; i < values.size(); ++i)
      {
        if (i % num_cols == 0)
        {
          result.resize(result.size() + 1);
        }
        result[i / num_cols].push_back(values[i]);
      }
      return result;
    }

    template <typename T>
    std::vector<T> vflatten(std::vector<std::vector<T>> values)
    {
      size_t total_size = 0;
      for (std::size_t i = 0; i < values.size(); i++)
      {
        total_size += values[i].size();
      }

      std::vector<T> result = std::vector<T>(total_size);
      size_t cur_idx = 0;
      for (std::size_t i = 0; i < values.size(); i++)
      {
        for (std::size_t j = 0; j < values[i].size(); j++)
        {
          result[cur_idx] = values[i][j];
          cur_idx++;
        }
      }
      return result;
    }

    inline bool starts_with(std::string const &value, std::string const &prefix)
    {
      if (prefix.length() > value.length())
        return false;
      return value.rfind(prefix, 0) == 0;
    }

    inline bool starts_with(std::string const &value, std::initializer_list<std::string> prefixes)
    {
      for (auto prefix : prefixes)
      {
        if (starts_with(value, prefix))
        {
          return true;
        }
      }
      return false;
    }

    inline bool file_exists(const std::string &name)
    {
      if (FILE *file = fopen(name.c_str(), "r"))
      {
        fclose(file);
        return true;
      }
      else
      {
        return false;
      }
    }

    std::string fullpath(std::initializer_list<std::string> path_components, std::string path_sep = std::string("/"))
    {
      std::string fp;
      if (path_components.size() == 0)
      {
        throw std::invalid_argument("The 'path_components' must not be empty.");
      }

      std::string comp;
      std::string comp_mod;
      size_t idx = 0;
      for (auto comp : path_components)
      {
        comp_mod = comp;
        if (idx != 0)
        { // We keep a leading slash intact for the first element (absolute path).
          if (starts_with(comp, path_sep))
          {
            comp_mod = comp.substr(1, comp.size() - 1);
          }
        }

        if (ends_with(comp_mod, path_sep))
        {
          comp_mod = comp_mod.substr(0, comp_mod.size() - 1);
        }

        fp += comp_mod;
        if (idx < path_components.size() - 1)
        {
          fp += path_sep;
        }
        idx++;
      }
      return fp;
    }

    void str_to_file(const std::string &filename, const std::string rep)
    {
      std::ofstream ofs;
      ofs.open(filename, std::ofstream::out);
#ifdef LIBFS_DBG_VERBOSE
      std::cout << LIBFS_APPTAG << "Opening file '" << filename << "' for writing.\n";
#endif
      if (ofs.is_open())
      {
        ofs << rep;
        ofs.close();
      }
      else
      {
        throw std::runtime_error("Unable to open file '" + filename + "' for writing.\n");
      }
    }

    std::vector<uint8_t> viridis(const std::vector<float> &data, float vmin = NAN, float vmax = NAN, uint8_t nan_r = 255, uint8_t nan_g = 255, uint8_t nan_b = 255)
    {
      std::vector<uint8_t> colors;
      if (data.empty())
      {
        return colors;
      }
      colors.reserve(data.size() * 3);

      // The official 256-entry Viridis colormap (RGB, floats in [0, 1]), identical to the
      // matplotlib viridis lookup table. Linearly interpolated between samples below.
      static const float lut[768] = {
      0.267004, 0.004874, 0.329415, 0.26851, 0.009605, 0.335427, 0.269944, 0.014625,
      0.341379, 0.271305, 0.019942, 0.347269, 0.272594, 0.025563, 0.353093, 0.273809,
      0.031497, 0.358853, 0.274952, 0.037752, 0.364543, 0.276022, 0.044167, 0.370164,
      0.277018, 0.050344, 0.375715, 0.277941, 0.056324, 0.381191, 0.278791, 0.062145,
      0.386592, 0.279566, 0.067836, 0.391917, 0.280267, 0.073417, 0.397163, 0.280894,
      0.078907, 0.402329, 0.281446, 0.08432, 0.407414, 0.281924, 0.089666, 0.412415,
      0.282327, 0.094955, 0.417331, 0.282656, 0.100196, 0.42216, 0.28291, 0.105393,
      0.426902, 0.283091, 0.110553, 0.431554, 0.283197, 0.11568, 0.436115, 0.283229,
      0.120777, 0.440584, 0.283187, 0.125848, 0.44496, 0.283072, 0.130895, 0.449241,
      0.282884, 0.13592, 0.453427, 0.282623, 0.140926, 0.457517, 0.28229, 0.145912,
      0.46151, 0.281887, 0.150881, 0.465405, 0.281412, 0.155834, 0.469201, 0.280868,
      0.160771, 0.472899, 0.280255, 0.165693, 0.476498, 0.279574, 0.170599, 0.479997,
      0.278826, 0.17549, 0.483397, 0.278012, 0.180367, 0.486697, 0.277134, 0.185228,
      0.489898, 0.276194, 0.190074, 0.493001, 0.275191, 0.194905, 0.496005, 0.274128,
      0.199721, 0.498911, 0.273006, 0.20452, 0.501721, 0.271828, 0.209303, 0.504434,
      0.270595, 0.214069, 0.507052, 0.269308, 0.218818, 0.509577, 0.267968, 0.223549,
      0.512008, 0.26658, 0.228262, 0.514349, 0.265145, 0.232956, 0.516599, 0.263663,
      0.237631, 0.518762, 0.262138, 0.242286, 0.520837, 0.260571, 0.246922, 0.522828,
      0.258965, 0.251537, 0.524736, 0.257322, 0.25613, 0.526563, 0.255645, 0.260703,
      0.528312, 0.253935, 0.265254, 0.529983, 0.252194, 0.269783, 0.531579, 0.250425,
      0.27429, 0.533103, 0.248629, 0.278775, 0.534556, 0.246811, 0.283237, 0.535941,
      0.244972, 0.287675, 0.53726, 0.243113, 0.292092, 0.538516, 0.241237, 0.296485,
      0.539709, 0.239346, 0.300855, 0.540844, 0.237441, 0.305202, 0.541921, 0.235526,
      0.309527, 0.542944, 0.233603, 0.313828, 0.543914, 0.231674, 0.318106, 0.544834,
      0.229739, 0.322361, 0.545706, 0.227802, 0.326594, 0.546532, 0.225863, 0.330805,
      0.547314, 0.223925, 0.334994, 0.548053, 0.221989, 0.339161, 0.548752, 0.220057,
      0.343307, 0.549413, 0.21813, 0.347432, 0.550038, 0.21621, 0.351535, 0.550627,
      0.214298, 0.355619, 0.551184, 0.212395, 0.359683, 0.55171, 0.210503, 0.363727,
      0.552206, 0.208623, 0.367752, 0.552675, 0.206756, 0.371758, 0.553117, 0.204903,
      0.375746, 0.553533, 0.203063, 0.379716, 0.553925, 0.201239, 0.38367, 0.554294,
      0.19943, 0.387607, 0.554642, 0.197636, 0.391528, 0.554969, 0.19586, 0.395433,
      0.555276, 0.1941, 0.399323, 0.555565, 0.192357, 0.403199, 0.555836, 0.190631,
      0.407061, 0.556089, 0.188923, 0.41091, 0.556326, 0.187231, 0.414746, 0.556547,
      0.185556, 0.41857, 0.556753, 0.183898, 0.422383, 0.556944, 0.182256, 0.426184,
      0.55712, 0.180629, 0.429975, 0.557282, 0.179019, 0.433756, 0.55743, 0.177423,
      0.437527, 0.557565, 0.175841, 0.44129, 0.557685, 0.174274, 0.445044, 0.557792,
      0.172719, 0.448791, 0.557885, 0.171176, 0.45253, 0.557965, 0.169646, 0.456262,
      0.55803, 0.168126, 0.459988, 0.558082, 0.166617, 0.463708, 0.558119, 0.165117,
      0.467423, 0.558141, 0.163625, 0.471133, 0.558148, 0.162142, 0.474838, 0.55814,
      0.160665, 0.47854, 0.558115, 0.159194, 0.482237, 0.558073, 0.157729, 0.485932,
      0.558013, 0.15627, 0.489624, 0.557936, 0.154815, 0.493313, 0.55784, 0.153364,
      0.497, 0.557724, 0.151918, 0.500685, 0.557587, 0.150476, 0.504369, 0.55743,
      0.149039, 0.508051, 0.55725, 0.147607, 0.511733, 0.557049, 0.14618, 0.515413,
      0.556823, 0.144759, 0.519093, 0.556572, 0.143343, 0.522773, 0.556295, 0.141935,
      0.526453, 0.555991, 0.140536, 0.530132, 0.555659, 0.139147, 0.533812, 0.555298,
      0.13777, 0.537492, 0.554906, 0.136408, 0.541173, 0.554483, 0.135066, 0.544853,
      0.554029, 0.133743, 0.548535, 0.553541, 0.132444, 0.552216, 0.553018, 0.131172,
      0.555899, 0.552459, 0.129933, 0.559582, 0.551864, 0.128729, 0.563265, 0.551229,
      0.127568, 0.566949, 0.550556, 0.126453, 0.570633, 0.549841, 0.125394, 0.574318,
      0.549086, 0.124395, 0.578002, 0.548287, 0.123463, 0.581687, 0.547445, 0.122606,
      0.585371, 0.546557, 0.121831, 0.589055, 0.545623, 0.121148, 0.592739, 0.544641,
      0.120565, 0.596422, 0.543611, 0.120092, 0.600104, 0.54253, 0.119738, 0.603785,
      0.5414, 0.119512, 0.607464, 0.540218, 0.119423, 0.611141, 0.538982, 0.119483,
      0.614817, 0.537692, 0.119699, 0.61849, 0.536347, 0.120081, 0.622161, 0.534946,
      0.120638, 0.625828, 0.533488, 0.12138, 0.629492, 0.531973, 0.122312, 0.633153,
      0.530398, 0.123444, 0.636809, 0.528763, 0.12478, 0.640461, 0.527068, 0.126326,
      0.644107, 0.525311, 0.128087, 0.647749, 0.523491, 0.130067, 0.651384, 0.521608,
      0.132268, 0.655014, 0.519661, 0.134692, 0.658636, 0.517649, 0.137339, 0.662252,
      0.515571, 0.14021, 0.665859, 0.513427, 0.143303, 0.669459, 0.511215, 0.146616,
      0.67305, 0.508936, 0.150148, 0.676631, 0.506589, 0.153894, 0.680203, 0.504172,
      0.157851, 0.683765, 0.501686, 0.162016, 0.687316, 0.499129, 0.166383, 0.690856,
      0.496502, 0.170948, 0.694384, 0.493803, 0.175707, 0.6979, 0.491033, 0.180653,
      0.701402, 0.488189, 0.185783, 0.704891, 0.485273, 0.19109, 0.708366, 0.482284,
      0.196571, 0.711827, 0.479221, 0.202219, 0.715272, 0.476084, 0.20803, 0.718701,
      0.472873, 0.214, 0.722114, 0.469588, 0.220124, 0.725509, 0.466226, 0.226397,
      0.728888, 0.462789, 0.232815, 0.732247, 0.459277, 0.239374, 0.735588, 0.455688,
      0.24607, 0.73891, 0.452024, 0.252899, 0.742211, 0.448284, 0.259857, 0.745492,
      0.444467, 0.266941, 0.748751, 0.440573, 0.274149, 0.751988, 0.436601, 0.281477,
      0.755203, 0.432552, 0.288921, 0.758394, 0.428426, 0.296479, 0.761561, 0.424223,
      0.304148, 0.764704, 0.419943, 0.311925, 0.767822, 0.415586, 0.319809, 0.770914,
      0.411152, 0.327796, 0.77398, 0.40664, 0.335885, 0.777018, 0.402049, 0.344074,
      0.780029, 0.397381, 0.35236, 0.783011, 0.392636, 0.360741, 0.785964, 0.387814,
      0.369214, 0.788888, 0.382914, 0.377779, 0.791781, 0.377939, 0.386433, 0.794644,
      0.372886, 0.395174, 0.797475, 0.367757, 0.404001, 0.800275, 0.362552, 0.412913,
      0.803041, 0.357269, 0.421908, 0.805774, 0.35191, 0.430983, 0.808473, 0.346476,
      0.440137, 0.811138, 0.340967, 0.449368, 0.813768, 0.335384, 0.458674, 0.816363,
      0.329727, 0.468053, 0.818921, 0.323998, 0.477504, 0.821444, 0.318195, 0.487026,
      0.823929, 0.312321, 0.496615, 0.826376, 0.306377, 0.506271, 0.828786, 0.300362,
      0.515992, 0.831158, 0.294279, 0.525776, 0.833491, 0.288127, 0.535621, 0.835785,
      0.281908, 0.545524, 0.838039, 0.275626, 0.555484, 0.840254, 0.269281, 0.565498,
      0.84243, 0.262877, 0.575563, 0.844566, 0.256415, 0.585678, 0.846661, 0.249897,
      0.595839, 0.848717, 0.243329, 0.606045, 0.850733, 0.236712, 0.616293, 0.852709,
      0.230052, 0.626579, 0.854645, 0.223353, 0.636902, 0.856542, 0.21662, 0.647257,
      0.8584, 0.209861, 0.657642, 0.860219, 0.203082, 0.668054, 0.861999, 0.196293,
      0.678489, 0.863742, 0.189503, 0.688944, 0.865448, 0.182725, 0.699415, 0.867117,
      0.175971, 0.709898, 0.868751, 0.169257, 0.720391, 0.87035, 0.162603, 0.730889,
      0.871916, 0.156029, 0.741388, 0.873449, 0.149561, 0.751884, 0.874951, 0.143228,
      0.762373, 0.876424, 0.137064, 0.772852, 0.877868, 0.131109, 0.783315, 0.879285,
      0.125405, 0.79376, 0.880678, 0.120005, 0.804182, 0.882046, 0.114965, 0.814576,
      0.883393, 0.110347, 0.82494, 0.88472, 0.106217, 0.83527, 0.886029, 0.102646,
      0.845561, 0.887322, 0.099702, 0.85581, 0.888601, 0.097452, 0.866013, 0.889868,
      0.095953, 0.876168, 0.891125, 0.09525, 0.886271, 0.892374, 0.095374, 0.89632,
      0.893616, 0.096335, 0.906311, 0.894855, 0.098125, 0.916242, 0.896091, 0.100717,
      0.926106, 0.89733, 0.104071, 0.935904, 0.89857, 0.108131, 0.945636, 0.899815,
      0.112838, 0.9553, 0.901065, 0.118128, 0.964894, 0.902323, 0.123941, 0.974417,
      0.90359, 0.130215, 0.983868, 0.904867, 0.136897, 0.993248, 0.906157, 0.143936,
    };

      const int n = 256;

      bool auto_min = std::isnan(vmin);
      bool auto_max = std::isnan(vmax);

      // Determine the finite (non-NaN) min/max of the data, used for auto range.
      float data_min = NAN;
      float data_max = NAN;
      bool have_finite = false;
      for (size_t i = 0; i < data.size(); i++)
      {
        if (std::isnan(data[i]))
        {
          continue;
        }
        if (!have_finite)
        {
          data_min = data[i];
          data_max = data[i];
          have_finite = true;
        }
        else
        {
          if (data[i] < data_min)
          {
            data_min = data[i];
          }
          if (data[i] > data_max)
          {
            data_max = data[i];
          }
        }
      }

      float lo = auto_min ? data_min : vmin;
      float hi = auto_max ? data_max : vmax;

      if (!auto_min && !auto_max)
      {
        if (vmin > vmax)
        {
          throw std::invalid_argument("In viridis(): 'vmin' must not be greater than 'vmax'.");
        }
      }

      if (!have_finite)
      {
        // All input values are NaN: map the whole vector to the configured NaN color.
        for (size_t i = 0; i < data.size(); i++)
        {
          colors.push_back(nan_r);
          colors.push_back(nan_g);
          colors.push_back(nan_b);
        }
        return colors;
      }

      bool constant = (hi <= lo);

      for (size_t i = 0; i < data.size(); i++)
      {
        if (std::isnan(data[i]))
        {
          colors.push_back(nan_r);
          colors.push_back(nan_g);
          colors.push_back(nan_b);
          continue;
        }

        float t;
        if (constant)
        {
          t = 0.5f;
        }
        else
        {
          t = (data[i] - lo) / (hi - lo);
          if (t < 0.0f) { t = 0.0f; }
          if (t > 1.0f) { t = 1.0f; }
        }

        float pos = t * (n - 1);
        int idx0 = static_cast<int>(pos);
        if (idx0 < 0) { idx0 = 0; }
        if (idx0 > n - 2) { idx0 = n - 2; }
        int idx1 = idx0 + 1;
        float frac = pos - static_cast<float>(idx0);

        for (int c = 0; c < 3; c++)
        {
          float val = lut[idx0 * 3 + c] * (1.0f - frac) + lut[idx1 * 3 + c] * frac;
          int iv = static_cast<int>(val * 255.0f + 0.5f);
          if (iv < 0) { iv = 0; }
          if (iv > 255) { iv = 255; }
          colors.push_back(static_cast<uint8_t>(iv));
        }
      }
      return colors;
    }
  } // End namespace util.

  // MRI data types, used by the MGH functions.

  const int MRI_UCHAR = 0;

  const int MRI_INT = 1;

  const int MRI_FLOAT = 3;

  const int MRI_SHORT = 4;

  // Forward declarations.
  int _fread3(std::istream &);
  template <typename T>
  T _freadt(std::istream &);
  std::string _freadstringnewline(std::istream &);
  std::string _freadfixedlengthstring(std::istream &, size_t, bool);
  bool _ends_with(std::string const &fullString, std::string const &ending);
  size_t _vidx_2d(size_t, size_t, size_t);
  struct MghHeader;

  struct Mesh
  {

    Mesh(std::vector<float> cvertices, std::vector<int32_t> cfaces)
    {
      vertices = cvertices;
      faces = cfaces;
    }

    // Construct from 2D vectors (Nx3).
    Mesh(std::vector<std::vector<float>> cvertices, std::vector<std::vector<int32_t>> cfaces)
    {
      vertices = util::vflatten(cvertices);
      faces = util::vflatten(cfaces);
    }

    Mesh() {}

    std::vector<float> vertices; 
    std::vector<int32_t> faces;  

    static fs::Mesh construct_cube()
    {
      fs::Mesh mesh;
      mesh.vertices = {1.0, 1.0, 1.0,
                       1.0, 1.0, -1.0,
                       1.0, -1.0, 1.0,
                       1.0, -1.0, -1.0,
                       -1.0, 1.0, 1.0,
                       -1.0, 1.0, -1.0,
                       -1.0, -1.0, 1.0,
                       -1.0, -1.0, -1.0};
      mesh.faces = {0, 2, 3,
                    3, 1, 0,
                    4, 6, 7,
                    7, 5, 4,
                    0, 4, 5,
                    5, 1, 0,
                    2, 6, 7,
                    7, 3, 2,
                    0, 4, 6,
                    6, 2, 0,
                    1, 5, 7,
                    7, 3, 1};
      return mesh;
    }

    static fs::Mesh construct_pyramid()
    {
      fs::Mesh mesh;
      mesh.vertices = {0.0, 0.0, 0.0, // start with 4x base
                       0.0, 1.0, 0.0,
                       1.0, 1.0, 0.0,
                       1.0, 0.0, 0.0,
                       0.5, 0.5, 1.0}; // apex
      mesh.faces = {0, 2, 1,           // start with 2 base faces
                    0, 3, 2,
                    0, 4, 1, // now the 4 wall faces
                    1, 4, 2,
                    3, 2, 4,
                    0, 3, 4};
      return mesh;
    }

    static fs::Mesh construct_grid(const size_t nx = 4, const size_t ny = 5, const float distx = 1.0, const float disty = 1.0)
    {
      if (nx < 2 || ny < 2)
      {
        throw std::runtime_error("Parameters nx and ny must be at least 2.");
      }
      fs::Mesh mesh;
      size_t num_vertices = nx * ny;
      size_t num_faces = ((nx - 1) * (ny - 1)) * 2;
      std::vector<float> vertices;
      vertices.reserve(num_vertices * 3);
      std::vector<int> faces;
      faces.reserve(num_faces * 3);

      // Create vertices.
      float cur_x, cur_y, cur_z;
      cur_x = cur_y = cur_z = 0.0;
      for (size_t i = 0; i < nx; i++)
      {
        for (size_t j = 0; j < ny; j++)
        {
          vertices.push_back(cur_x);
          vertices.push_back(cur_y);
          vertices.push_back(cur_z);
          cur_y += disty;
        }
        cur_x += distx;
      }

      // Create faces.
      for (size_t i = 0; i < num_vertices; i++)
      {
        if ((i + 1) % ny == 0 || i >= num_vertices - ny)
        {
          // Do not use the last ones in row or column as source.
          continue;
        }
        // Add the upper left triangle of this grid cell.
        faces.push_back(int(i));
        faces.push_back(int(i + ny + 1));
        faces.push_back(int(i + 1));
        // Add the lower right triangle of this grid cell.
        faces.push_back(int(i));
        faces.push_back(int(i + ny + 1));
        faces.push_back(int(i + ny));
      }

      mesh.vertices = vertices;
      mesh.faces = faces;
      return mesh;
    }

    std::string to_obj() const
    {
      std::stringstream objs;
      for (size_t vidx = 0; vidx < this->vertices.size(); vidx += 3)
      { // vertex coords
        objs << "v " << vertices[vidx] << " " << vertices[vidx + 1] << " " << vertices[vidx + 2] << "\n";
      }
      for (size_t fidx = 0; fidx < this->faces.size(); fidx += 3)
      { // faces: vertex indices, 1-based
        objs << "f " << faces[fidx] + 1 << " " << faces[fidx + 1] + 1 << " " << faces[fidx + 2] + 1 << "\n";
      }
      return (objs.str());
    }

    std::vector<std::vector<bool>> as_adjmatrix() const
    {
      std::vector<std::vector<bool>> adjm = std::vector<std::vector<bool>>(this->num_vertices(), std::vector<bool>(this->num_vertices(), false));
      for (size_t fidx = 0; fidx < this->faces.size(); fidx += 3)
      { // faces: vertex indices
        adjm[faces[fidx]][faces[fidx + 1]] = true;
        adjm[faces[fidx + 1]][faces[fidx]] = true;
        adjm[faces[fidx + 1]][faces[fidx + 2]] = true;
        adjm[faces[fidx + 2]][faces[fidx + 1]] = true;
        adjm[faces[fidx + 2]][faces[fidx]] = true;
        adjm[faces[fidx]][faces[fidx + 2]] = true;
      }
      return adjm;
    }

    struct _tupleHashFunction
    {
      size_t operator()(const std::tuple<size_t, size_t> &x) const
      {
        return std::get<0>(x) ^ std::get<1>(x);
      }
    };

    typedef std::unordered_set<std::tuple<size_t, size_t>, _tupleHashFunction> edge_set;

    edge_set as_edgelist() const
    {
      edge_set edges;
      for (size_t fidx = 0; fidx < this->faces.size(); fidx += 3)
      { // faces: vertex indices
        edges.insert(std::make_tuple(faces[fidx], faces[fidx + 1]));
        edges.insert(std::make_tuple(faces[fidx + 1], faces[fidx]));

        edges.insert(std::make_tuple(faces[fidx + 1], faces[fidx + 2]));
        edges.insert(std::make_tuple(faces[fidx + 2], faces[fidx + 1]));

        edges.insert(std::make_tuple(faces[fidx], faces[fidx + 2]));
        edges.insert(std::make_tuple(faces[fidx + 2], faces[fidx]));
      }
      return edges;
    }

    std::vector<std::vector<size_t>> as_adjlist(const bool via_matrix = true) const
    {
      if (!via_matrix)
      {
        return (this->_as_adjlist_via_edgeset());
      }
      std::vector<std::vector<bool>> adjm = this->as_adjmatrix();
      std::vector<std::vector<size_t>> adjl = std::vector<std::vector<size_t>>(this->num_vertices(), std::vector<size_t>());
      size_t nv = adjm.size();
      for (size_t i = 0; i < nv; i++)
      {
        for (size_t j = i + 1; j < nv; j++)
        {
          if (adjm[i][j] == true)
          {
            adjl[i].push_back(j);
            adjl[j].push_back(i);
          }
        }
      }
      return adjl;
    }

    std::vector<std::vector<size_t>> _as_adjlist_via_edgeset() const
    {
      edge_set edges = this->as_edgelist();
      std::vector<std::vector<size_t>> adjl = std::vector<std::vector<size_t>>(this->num_vertices(), std::vector<size_t>());
      for (const std::tuple<size_t, size_t> &e : edges)
      {
        adjl[std::get<0>(e)].push_back(std::get<1>(e));
      }
      return adjl;
    }

    std::vector<float> smooth_pvd_nn(const std::vector<float> pvd, const size_t num_iter = 1, const bool via_matrix = true, const bool with_nan = true, const bool detect_nan = true) const
    {

      const std::vector<std::vector<size_t>> adjlist = this->as_adjlist(via_matrix);
      return fs::Mesh::smooth_pvd_nn(adjlist, pvd, num_iter, with_nan, detect_nan);
    }

    static std::vector<float> smooth_pvd_nn(const std::vector<std::vector<size_t>> mesh_adj, const std::vector<float> pvd, const size_t num_iter = 1, const bool with_nan = true, const bool detect_nan = true)
    {
      assert(pvd.size() == mesh_adj.size());
      bool final_with_nan = with_nan;
      if (detect_nan)
      {
        final_with_nan = false;
        for (size_t i = 0; i < pvd.size(); i++)
        {
          if (std::isnan(pvd[i]))
          {
            final_with_nan = true;
            break;
          }
        }
      }
      if (final_with_nan)
      {
        return fs::Mesh::_smooth_pvd_nn_nan(mesh_adj, pvd, num_iter);
      }
      std::vector<float> current_pvd_source;
      std::vector<float> current_pvd_smoothed = std::vector<float>(pvd.size());

      float val_sum;
      size_t num_neigh;
      for (size_t i = 0; i < num_iter; i++)
      {
        if (i == 0)
        {
          current_pvd_source = pvd;
        }
        else
        {
          current_pvd_source = current_pvd_smoothed;
        }
        for (size_t v_idx = 0; v_idx < mesh_adj.size(); v_idx++)
        {
          num_neigh = mesh_adj[v_idx].size();
          val_sum = current_pvd_source[v_idx] / (num_neigh + 1);
          for (size_t neigh_rel_idx = 0; neigh_rel_idx < num_neigh; neigh_rel_idx++)
          {
            val_sum += current_pvd_source[mesh_adj[v_idx][neigh_rel_idx]] / (num_neigh + 1);
          }
          current_pvd_smoothed[v_idx] = val_sum;
        }
      }
      return current_pvd_smoothed;
    }

    static std::vector<float> _smooth_pvd_nn_nan(const std::vector<std::vector<size_t>> mesh_adj, const std::vector<float> pvd, const size_t num_iter = 1)
    {
      std::vector<float> current_pvd_source;
      std::vector<float> current_pvd_smoothed = std::vector<float>(pvd.size());

      float val_sum;
      size_t num_neigh;
      size_t num_non_nan_values;
      float neigh_val;
      for (size_t i = 0; i < num_iter; i++)
      {

        if (i == 0)
        {
          current_pvd_source = pvd;
        }
        else
        {
          current_pvd_source = current_pvd_smoothed;
        }

        for (size_t v_idx = 0; v_idx < mesh_adj.size(); v_idx++)
        {
          if (std::isnan(current_pvd_source[v_idx]))
          {
            current_pvd_smoothed[v_idx] = NAN;
            continue;
          }
          val_sum = current_pvd_source[v_idx];
          num_non_nan_values = 1; // If we get here, the source vertex value is not NAN.
          num_neigh = mesh_adj[v_idx].size();
          for (size_t neigh_rel_idx = 0; neigh_rel_idx < num_neigh; neigh_rel_idx++)
          {
            neigh_val = current_pvd_source[mesh_adj[v_idx][neigh_rel_idx]];
            if (std::isnan(neigh_val))
            {
              continue;
            }
            else
            {
              val_sum += neigh_val;
              num_non_nan_values++;
            }
          }
          current_pvd_smoothed[v_idx] = val_sum / (float)num_non_nan_values;
        }
      }
      return current_pvd_smoothed;
    }

    static std::vector<std::vector<size_t>> extend_adj(const std::vector<std::vector<size_t>> mesh_adj, const size_t extend_by = 1, std::vector<std::vector<size_t>> mesh_adj_ext = std::vector<std::vector<size_t>>())
    {
      size_t num_vertices = mesh_adj.size();
      if (mesh_adj_ext.size() == 0)
      {
        mesh_adj_ext = mesh_adj;
      }
      std::vector<size_t> neighborhood;
      std::vector<size_t> ext_neighborhood;
      for (size_t ext_idx = 0; ext_idx < extend_by; ext_idx++)
      {
        for (size_t source_vert_idx = 0; source_vert_idx < num_vertices; source_vert_idx++)
        {
          neighborhood = mesh_adj_ext[source_vert_idx]; // copy needed so we do not modify during iteration.
          // Extension: add all neighbors in distance one for all vertices in the neighborhood.
          for (size_t neigh_vert_rel_idx = 0; neigh_vert_rel_idx < neighborhood.size(); neigh_vert_rel_idx++)
          {
            for (size_t canidate_rel_idx = 0; canidate_rel_idx < mesh_adj[neighborhood[neigh_vert_rel_idx]].size(); canidate_rel_idx++)
            {
              if (mesh_adj[neighborhood[neigh_vert_rel_idx]][canidate_rel_idx] != source_vert_idx)
              {
                mesh_adj_ext[source_vert_idx].push_back(mesh_adj[neighborhood[neigh_vert_rel_idx]][canidate_rel_idx]);
              }
            }
          }
          // We need to remove duplicates.
          std::sort(mesh_adj_ext[source_vert_idx].begin(), mesh_adj_ext[source_vert_idx].end());
          mesh_adj_ext[source_vert_idx].erase(std::unique(mesh_adj_ext[source_vert_idx].begin(), mesh_adj_ext[source_vert_idx].end()), mesh_adj_ext[source_vert_idx].end());
        }
      }
      return mesh_adj_ext;
    }

    void to_obj_file(const std::string &filename) const
    {
      fs::util::str_to_file(filename, this->to_obj());
    }

    std::pair<std::unordered_map<int32_t, int32_t>, fs::Mesh> submesh_vertex(const std::vector<int32_t> &old_vertex_indices, const bool mapdir_fulltosubmesh = false) const
    {
      fs::Mesh submesh;
      std::vector<float> new_vertices;
      std::vector<int> new_faces;
      std::unordered_map<int32_t, int32_t> vertex_index_map_full2submesh;
      int32_t new_vertex_idx = 0;
      for (size_t i = 0; i < old_vertex_indices.size(); i++)
      {
        vertex_index_map_full2submesh[old_vertex_indices[i]] = new_vertex_idx;
        new_vertices.push_back(this->vertices[size_t(old_vertex_indices[i]) * 3]);
        new_vertices.push_back(this->vertices[size_t(old_vertex_indices[i]) * 3 + 1]);
        new_vertices.push_back(this->vertices[size_t(old_vertex_indices[i]) * 3 + 2]);
        new_vertex_idx++;
      }
      int face_v0;
      int face_v1;
      int face_v2;
      for (size_t i = 0; i < this->num_faces(); i++)
      {
        face_v0 = this->faces[i * 3];
        face_v1 = this->faces[i * 3 + 1];
        face_v2 = this->faces[i * 3 + 2];
        if ((vertex_index_map_full2submesh.find(face_v0) != vertex_index_map_full2submesh.end()) && (vertex_index_map_full2submesh.find(face_v1) != vertex_index_map_full2submesh.end()) && (vertex_index_map_full2submesh.find(face_v2) != vertex_index_map_full2submesh.end()))
        {
          new_faces.push_back(vertex_index_map_full2submesh[face_v0]);
          new_faces.push_back(vertex_index_map_full2submesh[face_v1]);
          new_faces.push_back(vertex_index_map_full2submesh[face_v2]);
        }
      }
      submesh.vertices = new_vertices;
      submesh.faces = new_faces;

      std::pair<std::unordered_map<int32_t, int32_t>, fs::Mesh> result;
      if (!mapdir_fulltosubmesh)
      { // Compute the new2old (reverse) vertex index map:
        std::unordered_map<int32_t, int32_t> vertex_index_map_submesh2full;
        for (auto const &pair : vertex_index_map_full2submesh)
        {
          vertex_index_map_submesh2full[pair.second] = pair.first;
        }
        result = std::pair<std::unordered_map<int32_t, int32_t>, fs::Mesh>(vertex_index_map_submesh2full, submesh);
      }
      else
      {
        result = std::pair<std::unordered_map<int32_t, int32_t>, fs::Mesh>(vertex_index_map_full2submesh, submesh);
      }

      return result;
    }

    static std::vector<float> curv_data_for_orig_mesh(const std::vector<float> data_submesh, const std::unordered_map<int32_t, int32_t> submesh_to_orig_mapping, const int32_t orig_mesh_num_vertices, const float fill_value = std::numeric_limits<float>::quiet_NaN())
    {

      if (submesh_to_orig_mapping.size() != data_submesh.size())
      {
        throw std::domain_error("The number of vertices of the submesh and the number of values in the submesh_to_orig_mapping do not match: got " + std::to_string(data_submesh.size()) + " and " + std::to_string(submesh_to_orig_mapping.size()) + ".");
      }

      std::vector<float> data_orig_mesh(orig_mesh_num_vertices, fill_value);
      for (size_t i = 0; i < data_submesh.size(); i++)
      {
        auto got = submesh_to_orig_mapping.find(int(i));
        if (got != submesh_to_orig_mapping.end())
        {
          data_orig_mesh[got->second] = data_submesh[i];
        }
      }
      return (data_orig_mesh);
    }

    static void from_obj(Mesh *mesh, std::istream *is)
    {
      std::string line;
      int line_idx = -1;

      std::vector<float> vertices;
      std::vector<int> faces;

#ifdef LIBFS_DBG_INFO
      size_t num_lines_ignored = 0; // Not comments, but custom extensions or material data lines which are ignored by libfs.
#endif

      while (std::getline(*is, line))
      {
        line_idx += 1;
        std::istringstream iss(line);
        if (fs::util::starts_with(line, "#"))
        {
          continue; // skip comment.
        }
        else
        {
          if (fs::util::starts_with(line, "v "))
          {
            std::string elem_type_identifier;
            float x, y, z;
            if (!(iss >> elem_type_identifier >> x >> y >> z))
            {
              throw std::domain_error("Could not parse vertex line " + std::to_string(line_idx + 1) + " of OBJ data, invalid format.\n");
            }
            assert(elem_type_identifier == "v");
            vertices.push_back(x);
            vertices.push_back(y);
            vertices.push_back(z);
          }
          else if (fs::util::starts_with(line, "f "))
          {
            std::string elem_type_identifier, v0raw, v1raw, v2raw;
            int v0, v1, v2;
            if (!(iss >> elem_type_identifier >> v0raw >> v1raw >> v2raw))
            {
              throw std::domain_error("Could not parse face line " + std::to_string(line_idx + 1) + " of OBJ data, invalid format.\n");
            }
            assert(elem_type_identifier == "f");

            // The OBJ format allows to specifiy face indices with slashes to also set normal and material indices.
            // So instead of a line like 'f 22 34 45', we could get 'f 3/1 4/2 5/3' or 'f 6/4/1 3/5/3 7/6/5' or 'f 7//1 8//2 9//3'.
            // We need to extract the stuff before the first slash and interprete it as int to get the vertex index we are looking for.
            std::size_t found_v0 = v0raw.find("/");
            std::size_t found_v1 = v1raw.find("/");
            std::size_t found_v2 = v2raw.find("/");
            if (found_v0 != std::string::npos)
            {
              v0raw = v0raw.substr(0, found_v0);
            }
            if (found_v1 != std::string::npos)
            {
              v1raw = v1raw.substr(0, found_v1);
            }
            if (found_v2 != std::string::npos)
            {
              v2raw = v0raw.substr(0, found_v2);
            }
            v0 = std::stoi(v0raw);
            v1 = std::stoi(v1raw);
            v2 = std::stoi(v2raw);

            // The vertex indices in Wavefront OBJ files are 1-based, so we have to substract 1 here.
            faces.push_back(v0 - 1);
            faces.push_back(v1 - 1);
            faces.push_back(v2 - 1);
          }
          else
          {
#ifdef LIBFS_DBG_INFO
            num_lines_ignored++;
#endif

            continue;
          }
        }
      }
#ifdef LIBFS_DBG_INFO
      if (num_lines_ignored > 0)
      {
        std::cout << LIBFS_APPTAG << "Ignored " << num_lines_ignored << " lines in Wavefront OBJ format mesh file.\n";
      }
#endif
      mesh->vertices = vertices;
      mesh->faces = faces;
    }

    static void from_obj(Mesh *mesh, const std::string &filename)
    {
#ifdef LIBFS_DBG_INFO
      std::cout << LIBFS_APPTAG << "Reading brain mesh from Wavefront object format file " << filename << ".\n";
#endif
      std::ifstream input(filename, std::fstream::in);
      if (input.is_open())
      {
        Mesh::from_obj(mesh, &input);
        input.close();
      }
      else
      {
        throw std::runtime_error("Could not open Wavefront object format mesh file '" + filename + "' for reading.\n");
      }
    }

    static void from_off(Mesh *mesh, std::istream *is, const std::string &source_filename = "")
    {

      std::string msg_source_file_part = source_filename.empty() ? "" : "'" + source_filename + "'";

      std::string line;
      int line_idx = -1;
      int noncomment_line_idx = -1;

      std::vector<float> vertices;
      std::vector<int> faces;
      size_t num_vertices = 0;
      size_t num_faces = 0;
      size_t num_edges = 0;
      size_t num_verts_parsed = 0;
      size_t num_faces_parsed = 0;
      float x, y, z; // vertex xyz coords
      // bool has_color;
      // int r, g, b, a;   // vertex colors
      int num_verts_this_face, v0, v1, v2; // face, defined by number of vertices and vertex indices.

      while (std::getline(*is, line))
      {
        line_idx++;
        std::istringstream iss(line);
        if (fs::util::starts_with(line, "#"))
        {
          continue; // skip comment.
        }
        else
        {
          noncomment_line_idx++;
          if (noncomment_line_idx == 0)
          {
            std::string off_header_magic;
            if (!(iss >> off_header_magic))
            {
              throw std::domain_error("Could not parse first header line " + std::to_string(line_idx + 1) + " of OFF data, invalid format.\n");
            }
            if (!(off_header_magic == "OFF" || off_header_magic == "COFF"))
            {
              throw std::domain_error("OFF magic string invalid, file " + msg_source_file_part + " not in OFF format.\n");
            }
            // has_color = off_header_magic == "COFF";
          }
          else if (noncomment_line_idx == 1)
          {
            if (!(iss >> num_vertices >> num_faces >> num_edges))
            {
              throw std::domain_error("Could not parse element count header line " + std::to_string(line_idx + 1) + " of OFF data " + msg_source_file_part + ", invalid format.\n");
            }
          }
          else
          {

            if (num_verts_parsed < num_vertices)
            {
              if (!(iss >> x >> y >> z))
              {
                throw std::domain_error("Could not parse vertex coordinate line " + std::to_string(line_idx + 1) + " of OFF data " + msg_source_file_part + ", invalid format.\n");
              }
              vertices.push_back(x);
              vertices.push_back(y);
              vertices.push_back(z);
              num_verts_parsed++;
            }
            else
            {
              if (num_faces_parsed < num_faces)
              {
                if (!(iss >> num_verts_this_face >> v0 >> v1 >> v2))
                {
                  throw std::domain_error("Could not parse face line " + std::to_string(line_idx + 1) + " of OFF data " + msg_source_file_part + ", invalid format.\n");
                }
                if (num_verts_this_face != 3)
                {
                  throw std::domain_error("At OFF data " + msg_source_file_part + " line " + std::to_string(line_idx + 1) + ": only triangular meshes supported.\n");
                }
                faces.push_back(v0);
                faces.push_back(v1);
                faces.push_back(v2);
                num_faces_parsed++;
              }
            }
          }
        }
      }
      if (num_verts_parsed < num_vertices)
      {
        throw std::domain_error("Vertex count mismatch between OFF data " + msg_source_file_part + " header (" + std::to_string(num_vertices) + ") and data (" + std::to_string(num_verts_parsed) + ").\n");
      }
      if (num_faces_parsed < num_faces)
      {
        throw std::domain_error("Face count mismatch between OFF data " + msg_source_file_part + " header  (" + std::to_string(num_faces) + ") and data (" + std::to_string(num_faces_parsed) + ").\n");
      }
      mesh->vertices = vertices;
      mesh->faces = faces;
    }

    static void from_off(Mesh *mesh, const std::string &filename)
    {
#ifdef LIBFS_DBG_INFO
      std::cout << LIBFS_APPTAG << "Reading brain mesh from OFF format file " << filename << ".\n";
#endif
      std::ifstream input(filename, std::fstream::in);
      if (input.is_open())
      {
        Mesh::from_off(mesh, &input);
        input.close();
      }
      else
      {
        throw std::runtime_error("Could not open Object file format (OFF) mesh file '" + filename + "' for reading.\n");
      }
    }

    static void from_ply(Mesh *mesh, std::istream *is)
    {
      std::string line;
      int line_idx = -1;
      int noncomment_line_idx = -1;

      std::vector<float> vertices;
      std::vector<int> faces;

      bool in_header = true; // current status
      int num_verts = -1;
      int num_faces = -1;
      while (std::getline(*is, line))
      {
        line_idx += 1;
        std::istringstream iss(line);
        if (fs::util::starts_with(line, "comment"))
        {
          continue; // skip comment.
        }
        else
        {
          noncomment_line_idx++;
          if (in_header)
          {
            if (noncomment_line_idx == 0)
            {
              if (line != "ply")
                throw std::domain_error("Invalid PLY file");
            }
            else if (noncomment_line_idx == 1)
            {
              if (line != "format ascii 1.0")
                throw std::domain_error("Unsupported PLY file format, only format 'format ascii 1.0' is supported.");
            }

            if (line == "end_header")
            {
              in_header = false;
            }
            else if (fs::util::starts_with(line, "element vertex"))
            {
              std::string elem, elem_type_identifier;
              if (!(iss >> elem >> elem_type_identifier >> num_verts))
              {
                throw std::domain_error("Could not parse element vertex line of PLY header, invalid format.\n");
              }
            }
            else if (fs::util::starts_with(line, "element face"))
            {
              std::string elem, elem_type_identifier;
              if (!(iss >> elem >> elem_type_identifier >> num_faces))
              {
                throw std::domain_error("Could not parse element face line of PLY header, invalid format.\n");
              }
            } // Other properties like vertex colors and normals are ignored for now.
          }
          else
          { // in data part.
            if (num_verts < 1 || num_faces < 1)
            {
              throw std::domain_error("Invalid PLY file: missing element count lines of header.");
            }
            // Read vertices
            if (vertices.size() < (size_t)num_verts * 3)
            {
              float x, y, z;
              if (!(iss >> x >> y >> z))
              {
                throw std::domain_error("Could not parse vertex line " + std::to_string(line_idx) + " of PLY data, invalid format.\n");
              }
              vertices.push_back(x);
              vertices.push_back(y);
              vertices.push_back(z);
            }
            else
            {
              if (faces.size() < (size_t)num_faces * 3)
              {
                int verts_per_face, v0, v1, v2;
                if (!(iss >> verts_per_face >> v0 >> v1 >> v2))
                {
                  throw std::domain_error("Could not parse face line " + std::to_string(line_idx) + " of PLY data, invalid format.\n");
                }
                if (verts_per_face != 3)
                {
                  throw std::domain_error("Only triangular meshes are supported: PLY faces lines must contain exactly 3 vertex indices.\n");
                }
                faces.push_back(v0);
                faces.push_back(v1);
                faces.push_back(v2);
              }
            }
          }
        }
      }
      if (vertices.size() != (size_t)num_verts * 3)
      {
        std::cerr << "PLY header mentions " << num_verts << " vertices, but found " << vertices.size() / 3 << ".\n";
      }
      if (faces.size() != (size_t)num_faces * 3)
      {
        std::cerr << "PLY header mentions " << num_faces << " faces, but found " << faces.size() / 3 << ".\n";
      }
      mesh->vertices = vertices;
      mesh->faces = faces;
    }

    static void from_ply(Mesh *mesh, const std::string &filename)
    {
#ifdef LIBFS_DBG_INFO
      std::cout << LIBFS_APPTAG << "Reading brain mesh from PLY format file " << filename << ".\n";
#endif
      std::ifstream input(filename, std::fstream::in);
      if (input.is_open())
      {
        Mesh::from_ply(mesh, &input);
        input.close();
      }
      else
      {
        throw std::runtime_error("Could not open Stanford PLY format mesh file '" + filename + "' for reading.\n");
      }
    }

    size_t num_vertices() const
    {
      return (this->vertices.size() / 3);
    }

    size_t num_faces() const
    {
      return (this->faces.size() / 3);
    }

    const int32_t &fm_at(const size_t i, const size_t j) const
    {
      size_t idx = _vidx_2d(i, j, 3);
      if (idx > this->faces.size() - 1)
      {
        throw std::range_error("Indices (" + std::to_string(i) + "," + std::to_string(j) + ") into Mesh.faces out of bounds. Hit " + std::to_string(idx) + " with max valid index " + std::to_string(this->faces.size() - 1) + ".\n");
      }
      return (this->faces[idx]);
    }

    std::vector<int32_t> face_vertices(const size_t face) const
    {
      if (face > this->num_faces() - 1)
      {
        throw std::range_error("Index " + std::to_string(face) + " into Mesh.faces out of bounds, max valid index is " + std::to_string(this->num_faces() - 1) + ".\n");
      }
      std::vector<int32_t> fv(3);
      fv[0] = this->fm_at(face, 0);
      fv[1] = this->fm_at(face, 1);
      fv[2] = this->fm_at(face, 2);
      return (fv);
    }

    std::vector<float> vertex_coords(const size_t vertex) const
    {
      if (vertex > this->num_vertices() - 1)
      {
        throw std::range_error("Index " + std::to_string(vertex) + " into Mesh.vertices out of bounds, max valid index is " + std::to_string(this->num_vertices() - 1) + ".\n");
      }
      std::vector<float> vc(3);
      vc[0] = this->vm_at(vertex, 0);
      vc[1] = this->vm_at(vertex, 1);
      vc[2] = this->vm_at(vertex, 2);
      return (vc);
    }

    const float &vm_at(const size_t i, const size_t j) const
    {
      size_t idx = _vidx_2d(i, j, 3);
      if (idx > this->vertices.size() - 1)
      {
        throw std::range_error("Indices (" + std::to_string(i) + "," + std::to_string(j) + ") into Mesh.vertices out of bounds. Hit " + std::to_string(idx) + " with max valid index " + std::to_string(this->vertices.size() - 1) + ".\n");
      }
      return (this->vertices[idx]);
    }

    std::string to_ply() const
    {
      std::vector<uint8_t> empty_col;
      return (this->to_ply(empty_col));
    }

    std::string to_ply(const std::vector<uint8_t> col) const
    {
      bool use_vertex_colors = col.size() != 0;
      std::stringstream plys;
      plys << "ply\nformat ascii 1.0\n";
      plys << "element vertex " << this->num_vertices() << "\n";
      plys << "property float x\nproperty float y\nproperty float z\n";
      if (use_vertex_colors)
      {
        if (col.size() != this->vertices.size())
        {
          throw std::invalid_argument("Number of vertex coordinates and vertex colors must match when writing PLY file, but got " + std::to_string(this->vertices.size()) + " and " + std::to_string(col.size()) + ".");
        }
        plys << "property uchar red\nproperty uchar green\nproperty uchar blue\n";
      }
      plys << "element face " << this->num_faces() << "\n";
      plys << "property list uchar int vertex_index\n";
      plys << "end_header\n";

#ifdef LIBFS_DBG_DEBUG
      fs::util::log("Writing " + std::to_string(this->vertices.size() / 3) + " PLY format vertices.", "INFO");
#endif

      for (size_t vidx = 0; vidx < this->vertices.size(); vidx += 3)
      { // vertex coords
        plys << vertices[vidx] << " " << vertices[vidx + 1] << " " << vertices[vidx + 2];
        if (use_vertex_colors)
        {
          plys << " " << (int)col[vidx] << " " << (int)col[vidx + 1] << " " << (int)col[vidx + 2];
        }
        plys << "\n";
      }

#ifdef LIBFS_DBG_DEBUG
      fs::util::log("Writing " + std::to_string(this->faces.size() / 3) + " PLY format faces.", "INFO");
#endif

      const int num_vertices_per_face = 3;
      for (size_t fidx = 0; fidx < this->faces.size(); fidx += 3)
      { // faces: vertex indices, 0-based
        plys << num_vertices_per_face << " " << faces[fidx] << " " << faces[fidx + 1] << " " << faces[fidx + 2] << "\n";
      }
      return (plys.str());
    }

    void to_ply_file(const std::string &filename) const
    {
#ifdef LIBFS_DBG_INFO
      fs::util::log("Writing mesh to PLY file '" + filename + "'.", "INFO");
#endif
      fs::util::str_to_file(filename, this->to_ply());
    }

    void to_ply_file(const std::string &filename, const std::vector<uint8_t> col) const
    {
      fs::util::str_to_file(filename, this->to_ply(col));
    }

    std::string to_off() const
    {
      std::vector<uint8_t> empty_col;
      return (this->to_off(empty_col));
    }

    std::string to_off(const std::vector<uint8_t> col) const
    {
      bool use_vertex_colors = col.size() != 0;
      std::stringstream offs;
      if (use_vertex_colors)
      {
#ifdef LIBFS_DBG_INFO
        fs::util::log("Writing OFF representation of mesh with vertex colors.", "INFO");
#endif
        if (col.size() != this->vertices.size())
        {
          throw std::invalid_argument("Number of vertex coordinates and vertex colors must match when writing OFF file but got " + std::to_string(this->vertices.size()) + " and " + std::to_string(col.size()) + ".");
        }
        offs << "COFF\n";
      }
      else
      {
#ifdef LIBFS_DBG_INFO
        fs::util::log("Writing OFF representation of mesh without vertex colors.", "INFO");
#endif
        offs << "OFF\n";
      }
      offs << this->num_vertices() << " " << this->num_faces() << " 0\n";

      for (size_t vidx = 0; vidx < this->vertices.size(); vidx += 3)
      { // vertex coords
        offs << vertices[vidx] << " " << vertices[vidx + 1] << " " << vertices[vidx + 2];
        if (use_vertex_colors)
        {
          offs << " " << (int)col[vidx] << " " << (int)col[vidx + 1] << " " << (int)col[vidx + 2] << " 255";
        }
        offs << "\n";
      }

      const int num_vertices_per_face = 3;
      for (size_t fidx = 0; fidx < this->faces.size(); fidx += 3)
      { // faces: vertex indices, 0-based
        offs << num_vertices_per_face << " " << faces[fidx] << " " << faces[fidx + 1] << " " << faces[fidx + 2] << "\n";
      }
      return (offs.str());
    }

    void to_off_file(const std::string &filename) const
    {
      fs::util::str_to_file(filename, this->to_off());
    }

    void to_off_file(const std::string &filename, const std::vector<uint8_t> col) const
    {
      fs::util::str_to_file(filename, this->to_off(col));
    }
  };

  struct Curv
  {

    Curv(std::vector<float> curv_data) : num_faces(100000), num_vertices(0), num_values_per_vertex(1)
    {
      data = curv_data;
      num_vertices = int(data.size());
    }

    Curv() : num_faces(100000), num_vertices(0), num_values_per_vertex(1) {}

    int32_t num_faces;

    std::vector<float> data;

    int32_t num_vertices;

    int32_t num_values_per_vertex;
  };

  struct Colortable
  {
    std::vector<int32_t> id;       
    std::vector<std::string> name; 
    std::vector<int32_t> r;        
    std::vector<int32_t> g;        
    std::vector<int32_t> b;        
    std::vector<int32_t> a;        
    std::vector<int32_t> label;    

    size_t num_entries() const
    {
      size_t num_ids = this->id.size();
      if (this->name.size() != num_ids || this->r.size() != num_ids || this->g.size() != num_ids || this->b.size() != num_ids || this->a.size() != num_ids || this->label.size() != num_ids)
      {
        std::cerr << "Inconsistent Colortable, vector sizes do not match.\n";
      }
      return num_ids;
    }

    int32_t get_region_idx(const std::string &query_name) const
    {
      for (size_t i = 0; i < this->num_entries(); i++)
      {
        if (this->name[i] == query_name)
        {
          return (int32_t)i;
        }
      }
      return (-1);
    }

    int32_t get_region_idx(int32_t query_label) const
    {
      for (size_t i = 0; i < this->num_entries(); i++)
      {
        if (this->label[i] == query_label)
        {
          return (int32_t)i;
        }
      }
      return (-1);
    }
  };

  struct Annot
  {
    std::vector<int32_t> vertex_indices; 
    std::vector<int32_t> vertex_labels;  
    Colortable colortable;               

    std::vector<int32_t> region_vertices(const std::string &region_name) const
    {
      int32_t region_idx = this->colortable.get_region_idx(region_name);
      if (region_idx >= 0)
      {
        return (this->region_vertices(this->colortable.label[region_idx]));
      }
      else
      {
        std::cerr << "No such region in annot, returning empty vector.\n";
        std::vector<int32_t> empty;
        return (empty);
      }
    }

    std::vector<int32_t> region_vertices(int32_t region_label) const
    {
      std::vector<int32_t> reg_verts;
      for (size_t i = 0; i < this->vertex_labels.size(); i++)
      {
        if (this->vertex_labels[i] == region_label)
        {
          reg_verts.push_back(int(i));
        }
      }
      return (reg_verts);
    }

    std::vector<uint8_t> vertex_colors(bool alpha = false) const
    {
      int num_channels = alpha ? 4 : 3;
      std::vector<uint8_t> col;
      col.reserve(this->num_vertices() * num_channels);
      std::vector<size_t> vertex_region_indices = this->vertex_regions();
      for (size_t i = 0; i < this->num_vertices(); i++)
      {
        col.push_back(this->colortable.r[vertex_region_indices[i]]);
        col.push_back(this->colortable.g[vertex_region_indices[i]]);
        col.push_back(this->colortable.b[vertex_region_indices[i]]);
        if (alpha)
        {
          col.push_back(this->colortable.a[vertex_region_indices[i]]);
        }
      }
      return (col);
    }

    size_t num_vertices() const
    {
      size_t nv = this->vertex_indices.size();
      if (this->vertex_labels.size() != nv)
      {
        throw std::runtime_error("Inconsistent annot, number of vertex indices and labels does not match.\n");
      }
      return nv;
    }

    std::vector<size_t> vertex_regions() const
    {
      std::vector<size_t> vert_reg;
      for (size_t i = 0; i < this->num_vertices(); i++)
      {
        vert_reg.push_back(0); // init with zeros.
      }
      for (size_t region_idx = 0; region_idx < this->colortable.num_entries(); region_idx++)
      {
        std::vector<int32_t> reg_vertices = this->region_vertices(this->colortable.label[region_idx]);
        for (size_t region_vert_local_idx = 0; region_vert_local_idx < reg_vertices.size(); region_vert_local_idx++)
        {
          int32_t region_vert_idx = reg_vertices[region_vert_local_idx];
          vert_reg[region_vert_idx] = region_idx;
        }
      }
      return vert_reg;
    }

    std::vector<std::string> vertex_region_names() const
    {
      std::vector<std::string> region_names;
      std::vector<size_t> vertex_region_indices = this->vertex_regions();
      for (size_t i = 0; i < this->num_vertices(); i++)
      {
        region_names.push_back(this->colortable.name[vertex_region_indices[i]]);
      }
      return (region_names);
    }
  };

  struct MghHeader
  {
    MghHeader() {} 
    MghHeader(Curv curv)
    { 
      dim1length = curv.data.size();
      dim2length = 1;
      dim3length = 1;
      dim4length = 1;
      dtype = fs::MRI_FLOAT;
    }
    MghHeader(std::vector<float> curv_data)
    { 
      dim1length = curv_data.size();
      dim2length = 1;
      dim3length = 1;
      dim4length = 1;
      dtype = fs::MRI_FLOAT;
    }
    int32_t dim1length = 0; 
    int32_t dim2length = 0; 
    int32_t dim3length = 0; 
    int32_t dim4length = 0; 

    int32_t dtype = 0;         
    int32_t dof = 0;           
    int16_t ras_good_flag = 0; 

    size_t num_values() const
    {
      return ((size_t)dim1length * dim2length * dim3length * dim4length);
    }

    float xsize = 0.0;         
    float ysize = 0.0;         
    float zsize = 0.0;         
    std::vector<float> Mdc;    
    std::vector<float> Pxyz_c; 
  };

  struct MghData
  {
    MghData() {}
    MghData(std::vector<int32_t> curv_data) { data_mri_int = curv_data; }            
    explicit MghData(std::vector<uint8_t> curv_data) { data_mri_uchar = curv_data; } 
    explicit MghData(std::vector<short> curv_data) { data_mri_short = curv_data; }   
    MghData(std::vector<float> curv_data) { data_mri_float = curv_data; }            
    MghData(Curv curv) { data_mri_float = curv.data; }                               
    std::vector<int32_t> data_mri_int;                                               
    std::vector<uint8_t> data_mri_uchar;                                             
    std::vector<float> data_mri_float;                                               
    std::vector<short> data_mri_short;                                               
  };

  struct Mgh
  {
    MghHeader header; 
    MghData data;     
    Mgh() {}          
    Mgh(Curv curv)
    { 
      header = MghHeader(curv);
      data = MghData(curv);
    }
    Mgh(std::vector<float> curv_data)
    { 
      header = MghHeader(curv_data);
      data = MghData(curv_data);
    }
  };

  template <class T>
  struct Array4D
  {
    Array4D(unsigned int d1, unsigned int d2, unsigned int d3, unsigned int d4) : d1(d1), d2(d2), d3(d3), d4(d4), data(d1 * d2 * d3 * d4) {}

    Array4D(MghHeader *mgh_header) : d1(mgh_header->dim1length), d2(mgh_header->dim2length), d3(mgh_header->dim3length), d4(mgh_header->dim4length), data(d1 * d2 * d3 * d4) {}

    Array4D(Mgh *mgh) : // This does NOT init the data atm.
                        d1(mgh->header.dim1length), d2(mgh->header.dim2length), d3(mgh->header.dim3length), d4(mgh->header.dim4length), data(d1 * d2 * d3 * d4)
    {
    }

    const T &at(const unsigned int i1, const unsigned int i2, const unsigned int i3, const unsigned int i4) const
    {
      return data[get_index(i1, i2, i3, i4)];
    }

    unsigned int get_index(const unsigned int i1, const unsigned int i2, const unsigned int i3, const unsigned int i4) const
    {
      assert(i1 >= 0 && i1 < d1);
      assert(i2 >= 0 && i2 < d2);
      assert(i3 >= 0 && i3 < d3);
      assert(i4 >= 0 && i4 < d4);
      return (((i1 * d2 + i2) * d3 + i3) * d4 + i4);
    }

    unsigned int num_values() const
    {
      return (d1 * d2 * d3 * d4);
    }

    unsigned int d1;     
    unsigned int d2;     
    unsigned int d3;     
    unsigned int d4;     
    std::vector<T> data; 
  };

  // More declarations, should also go to separate header.
  void read_mgh_header(MghHeader *, const std::string &);
  void read_mgh_header(MghHeader *, std::istream *);
  template <typename T>
  std::vector<T> _read_mgh_data(MghHeader *, const std::string &);
  template <typename T>
  std::vector<T> _read_mgh_data(MghHeader *, std::istream *);
  std::vector<int32_t> _read_mgh_data_int(MghHeader *, const std::string &);
  std::vector<int32_t> _read_mgh_data_int(MghHeader *, std::istream *);
  std::vector<uint8_t> _read_mgh_data_uchar(MghHeader *, const std::string &);
  std::vector<uint8_t> _read_mgh_data_uchar(MghHeader *, std::istream *);
  std::vector<short> _read_mgh_data_short(MghHeader *, const std::string &);
  std::vector<short> _read_mgh_data_short(MghHeader *, std::istream *);
  std::vector<float> _read_mgh_data_float(MghHeader *, const std::string &);
  std::vector<float> _read_mgh_data_float(MghHeader *, std::istream *);

  void read_mgh(Mgh *mgh, const std::string &filename)
  {
    MghHeader mgh_header;
    read_mgh_header(&mgh_header, filename);
    mgh->header = mgh_header;
    if (mgh->header.dtype == MRI_INT)
    {
      std::vector<int32_t> data = _read_mgh_data_int(&mgh_header, filename);
      mgh->data.data_mri_int = data;
    }
    else if (mgh->header.dtype == MRI_UCHAR)
    {
      std::vector<uint8_t> data = _read_mgh_data_uchar(&mgh_header, filename);
      mgh->data.data_mri_uchar = data;
    }
    else if (mgh->header.dtype == MRI_FLOAT)
    {
      std::vector<float> data = _read_mgh_data_float(&mgh_header, filename);
      mgh->data.data_mri_float = data;
    }
    else if (mgh->header.dtype == MRI_SHORT)
    {
      std::vector<short> data = _read_mgh_data_short(&mgh_header, filename);
      mgh->data.data_mri_short = data;
    }
    else
    {
#ifdef LIBFS_DBG_INFO
      if (fs::util::ends_with(filename, ".mgz"))
      {
        std::cout << LIBFS_APPTAG << "Note: your MGH filename ends with '.mgz'. Keep in mind that MGZ format is not supported directly. You can ignore this message if you wrapped a gz stream.\n";
      }
#endif
      throw std::runtime_error("Not reading MGH data from file '" + filename + "', data type " + std::to_string(mgh->header.dtype) + " not supported yet.\n");
    }
  }

  std::vector<std::string> read_subjectsfile(const std::string &filename)
  {
    std::vector<std::string> subjects;
    std::ifstream input(filename, std::fstream::in);
    std::string line;

    if (!input.is_open())
    {
      throw std::runtime_error("Could not open subjects file '" + filename + "'.\n");
    }

    while (std::getline(input, line))
    {
      subjects.push_back(line);
    }
    return (subjects);
  }

  void read_mgh(Mgh *mgh, std::istream *is)
  {
    MghHeader mgh_header;
    read_mgh_header(&mgh_header, is);
    mgh->header = mgh_header;
    if (mgh->header.dtype == MRI_INT)
    {
      std::vector<int32_t> data = _read_mgh_data_int(&mgh_header, is);
      mgh->data.data_mri_int = data;
    }
    else if (mgh->header.dtype == MRI_UCHAR)
    {
      std::vector<uint8_t> data = _read_mgh_data_uchar(&mgh_header, is);
      mgh->data.data_mri_uchar = data;
    }
    else if (mgh->header.dtype == MRI_FLOAT)
    {
      std::vector<float> data = _read_mgh_data_float(&mgh_header, is);
      mgh->data.data_mri_float = data;
    }
    else if (mgh->header.dtype == MRI_SHORT)
    {
      std::vector<short> data = _read_mgh_data_short(&mgh_header, is);
      mgh->data.data_mri_short = data;
    }
    else
    {
      throw std::runtime_error("Not reading data from MGH stream, data type " + std::to_string(mgh->header.dtype) + " not supported yet.\n");
    }
  }

  void read_mgh_header(MghHeader *mgh_header, std::istream *is)
  {
    const int MGH_VERSION = 1;

    int format_version = _freadt<int32_t>(*is);
    if (format_version != MGH_VERSION)
    {
      throw std::runtime_error("Invalid MGH file or unsupported file format version: expected version " + std::to_string(MGH_VERSION) + ", found " + std::to_string(format_version) + ".\n");
    }
    mgh_header->dim1length = _freadt<int32_t>(*is);
    mgh_header->dim2length = _freadt<int32_t>(*is);
    mgh_header->dim3length = _freadt<int32_t>(*is);
    mgh_header->dim4length = _freadt<int32_t>(*is);

    mgh_header->dtype = _freadt<int32_t>(*is);
    mgh_header->dof = _freadt<int32_t>(*is);

    int unused_header_space_size_left = 256; // in bytes
    mgh_header->ras_good_flag = _freadt<int16_t>(*is);
    unused_header_space_size_left -= 2; // for the ras_good_flag

    // Read the RAS part of the header.
    if (mgh_header->ras_good_flag == 1)
    {
      mgh_header->xsize = _freadt<float>(*is);
      mgh_header->ysize = _freadt<float>(*is);
      mgh_header->zsize = _freadt<float>(*is);

      for (int i = 0; i < 9; i++)
      {
        mgh_header->Mdc.push_back(_freadt<float>(*is));
      }
      for (int i = 0; i < 3; i++)
      {
        mgh_header->Pxyz_c.push_back(_freadt<float>(*is));
      }
      unused_header_space_size_left -= 60;
    }

    // Advance to data part. We do not seek here because that is not
    // possible if the stream is gzip-wrapped with zstr, as in the read_mgz example.
    uint8_t discarded;
    while (unused_header_space_size_left > 0)
    {
      discarded = _freadt<uint8_t>(*is);
      unused_header_space_size_left -= 1;
    }
    (void)discarded; // Suppress warnings about unused variable.
  }

  std::vector<int32_t> _read_mgh_data_int(MghHeader *mgh_header, const std::string &filename)
  {
    if (mgh_header->dtype != MRI_INT)
    {
      std::cerr << "Expected MRI data type " << MRI_INT << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<int32_t>(mgh_header, filename));
  }

  std::vector<int32_t> _read_mgh_data_int(MghHeader *mgh_header, std::istream *is)
  {
    if (mgh_header->dtype != MRI_INT)
    {
      std::cerr << "Expected MRI data type " << MRI_INT << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<int32_t>(mgh_header, is));
  }

  std::vector<short> _read_mgh_data_short(MghHeader *mgh_header, const std::string &filename)
  {
    if (mgh_header->dtype != MRI_SHORT)
    {
      std::cerr << "Expected MRI data type " << MRI_SHORT << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<short>(mgh_header, filename));
  }

  std::vector<short> _read_mgh_data_short(MghHeader *mgh_header, std::istream *is)
  {
    if (mgh_header->dtype != MRI_SHORT)
    {
      std::cerr << "Expected MRI data type " << MRI_SHORT << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<short>(mgh_header, is));
  }

  void read_mgh_header(MghHeader *mgh_header, const std::string &filename)
  {
    std::ifstream ifs;
    ifs.open(filename, std::ios_base::in | std::ios::binary);
    if (ifs.is_open())
    {
      read_mgh_header(mgh_header, &ifs);
      ifs.close();
    }
    else
    {
      throw std::runtime_error("Unable to open MGH file '" + filename + "'.\n");
    }
  }

  template <typename T>
  std::vector<T> _read_mgh_data(MghHeader *mgh_header, const std::string &filename)
  {
    std::ifstream ifs;
    ifs.open(filename, std::ios_base::in | std::ios::binary);
    if (ifs.is_open())
    {
      ifs.seekg(284, ifs.beg); // skip to end of header and beginning of data

      int num_values = int(mgh_header->num_values());
      std::vector<T> data;
      for (int i = 0; i < num_values; i++)
      {
        data.push_back(_freadt<T>(ifs));
      }
      ifs.close();
      return (data);
    }
    else
    {
      throw std::runtime_error("Unable to open MGH file '" + filename + "'.\n");
    }
  }

  template <typename T>
  std::vector<T> _read_mgh_data(MghHeader *mgh_header, std::istream *is)
  {
    int num_values = int(mgh_header->num_values());
    std::vector<T> data;
    for (int i = 0; i < num_values; i++)
    {
      data.push_back(_freadt<T>(*is));
    }
    return (data);
  }

  std::vector<float> _read_mgh_data_float(MghHeader *mgh_header, const std::string &filename)
  {
    if (mgh_header->dtype != MRI_FLOAT)
    {
      std::cerr << "Expected MRI data type " << MRI_FLOAT << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<float>(mgh_header, filename));
  }

  std::vector<float> _read_mgh_data_float(MghHeader *mgh_header, std::istream *is)
  {
    if (mgh_header->dtype != MRI_FLOAT)
    {
      std::cerr << "Expected MRI data type " << MRI_FLOAT << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<float>(mgh_header, is));
  }

  std::vector<uint8_t> _read_mgh_data_uchar(MghHeader *mgh_header, const std::string &filename)
  {
    if (mgh_header->dtype != MRI_UCHAR)
    {
      std::cerr << "Expected MRI data type " << MRI_UCHAR << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<uint8_t>(mgh_header, filename));
  }

  std::vector<uint8_t> _read_mgh_data_uchar(MghHeader *mgh_header, std::istream *is)
  {
    if (mgh_header->dtype != MRI_UCHAR)
    {
      std::cerr << "Expected MRI data type " << MRI_UCHAR << ", but found " << mgh_header->dtype << ".\n";
    }
    return (_read_mgh_data<uint8_t>(mgh_header, is));
  }

  void read_surf(Mesh *surface, const std::string &filename)
  {
    const int SURF_TRIS_MAGIC = 16777214;
    std::ifstream is;
    is.open(filename, std::ios_base::in | std::ios::binary);
    if (is.is_open())
    {
      int magic = _fread3(is);
      if (magic != SURF_TRIS_MAGIC)
      {
        throw std::domain_error("Surf file '" + filename + "' magic code in header did not match: expected " + std::to_string(SURF_TRIS_MAGIC) + ", found " + std::to_string(magic) + ".\n");
      }
      std::string created_line = _freadstringnewline(is);
      std::string comment_line = _freadstringnewline(is);
      int num_verts = _freadt<int32_t>(is);
      int num_faces = _freadt<int32_t>(is);
#ifdef LIBFS_DBG_INFO
      std::cout << LIBFS_APPTAG << "Read surface file with " << num_verts << " vertices, " << num_faces << " faces.\n";
#endif
      std::vector<float> vdata;
      for (int i = 0; i < (num_verts * 3); i++)
      {
        vdata.push_back(_freadt<float>(is));
      }
      std::vector<int> fdata;
      for (int i = 0; i < (num_faces * 3); i++)
      {
        fdata.push_back(_freadt<int32_t>(is));
      }
      is.close();
      surface->vertices = vdata;
      surface->faces = fdata;
    }
    else
    {
      throw std::runtime_error("Unable to open surface file '" + filename + "'.\n");
    }
  }

  void read_mesh(Mesh *surface, const std::string &filename)
  {
    if (fs::util::ends_with(filename, ".obj"))
    {
      fs::Mesh::from_obj(surface, filename);
    }
    else if (fs::util::ends_with(filename, ".ply"))
    {
      fs::Mesh::from_ply(surface, filename);
    }
    else if (fs::util::ends_with(filename, ".off"))
    {
      fs::Mesh::from_off(surface, filename);
    }
    else
    {
      read_surf(surface, filename);
    }
  }

  bool _is_bigendian()
  {
    short int number = 0x1;
    char *numPtr = (char *)&number;
    // std::cout << "Platform is big endian: " << (numPtr[0] != 1) << ".\n";
    return (numPtr[0] != 1);
  }

  void read_curv(Curv *curv, std::istream *is, const std::string &source_filename = "")
  {
    const std::string msg_source_file_part = source_filename.empty() ? "" : "'" + source_filename + "' ";
    const int CURV_MAGIC = 16777215;
    int magic = _fread3(*is);
    if (magic != CURV_MAGIC)
    {
      throw std::domain_error("Curv file " + msg_source_file_part + "header magic did not match: expected " + std::to_string(CURV_MAGIC) + ", found " + std::to_string(magic) + ".\n");
    }
    curv->num_vertices = _freadt<int32_t>(*is);
    curv->num_faces = _freadt<int32_t>(*is);
    curv->num_values_per_vertex = _freadt<int32_t>(*is);
#ifdef LIBFS_DBG_INFO
    std::cout << LIBFS_APPTAG << "Read curv file with " << curv->num_vertices << " vertices, " << curv->num_faces << " faces and " << curv->num_values_per_vertex << " values per vertex.\n";
#endif
    if (curv->num_values_per_vertex != 1)
    { // Not supported, I know no case where this is used. Please submit a PR with a demo file if you have one, and let me know where it came from.
      throw std::domain_error("Curv file " + msg_source_file_part + "must contain exactly 1 value per vertex, found " + std::to_string(curv->num_values_per_vertex) + ".\n");
    }
    std::vector<float> data;
    for (int i = 0; i < curv->num_vertices; i++)
    {
      data.push_back(_freadt<float>(*is));
    }
    curv->data = data;
  }

  void read_curv(Curv *curv, const std::string &filename)
  {
    std::ifstream is(filename, std::fstream::in | std::fstream::binary);
    if (is.is_open())
    {
      read_curv(curv, &is, filename);
      is.close();
    }
    else
    {
      throw std::runtime_error("Could not open curv file '" + filename + "' for reading.\n");
    }
  }

  void _read_annot_colortable(Colortable *colortable, std::istream *is, int32_t num_entries)
  {
    int32_t num_chars_orig_filename = _freadt<int32_t>(*is); // The number of characters of the file this annot was built from.

    // It follows the name of the file this annot was built from. This is development metadata and irrelevant afaik. We skip it.
    uint8_t discarded;
    for (int32_t i = 0; i < num_chars_orig_filename; i++)
    {
      discarded = _freadt<uint8_t>(*is);
    }
    (void)discarded; // Suppress warnings about unused variable.

    int32_t num_entries_duplicated = _freadt<int32_t>(*is); // Yes, once more.
    if (num_entries != num_entries_duplicated)
    {
      std::cerr << "Warning: the two num_entries header fields of this annotation do not match. Use with care.\n";
    }

    int32_t entry_num_chars;
    for (int32_t i = 0; i < num_entries; i++)
    {
      colortable->id.push_back(_freadt<int32_t>(*is));
      entry_num_chars = _freadt<int32_t>(*is);
      colortable->name.push_back(_freadfixedlengthstring(*is, entry_num_chars, true));
      colortable->r.push_back(_freadt<int32_t>(*is));
      colortable->g.push_back(_freadt<int32_t>(*is));
      colortable->b.push_back(_freadt<int32_t>(*is));
      colortable->a.push_back(_freadt<int32_t>(*is));
      colortable->label.push_back(colortable->r[i] + colortable->g[i] * 256 + colortable->b[i] * 65536 + colortable->a[i] * 16777216);
    }
  }

  size_t _vidx_2d(size_t row, size_t column, size_t row_length = 3)
  {
    return (row + 1) * row_length - row_length + column;
  }

  void read_annot(Annot *annot, std::istream *is)
  {

    int32_t num_vertices = _freadt<int32_t>(*is);
    std::vector<int32_t> vertices;
    std::vector<int32_t> labels;
    for (int32_t i = 0; i < (num_vertices * 2); i++)
    { // The vertices and their labels are stored directly after one another: v1,v1_label,v2,v2_label,...
      if (i % 2 == 0)
      {
        vertices.push_back(_freadt<int32_t>(*is));
      }
      else
      {
        labels.push_back(_freadt<int32_t>(*is));
      }
    }
    annot->vertex_indices = vertices;
    annot->vertex_labels = labels;
    int32_t has_colortable = _freadt<int32_t>(*is);
    if (has_colortable == 1)
    {
      int32_t num_colortable_entries_old_format = _freadt<int32_t>(*is);
      if (num_colortable_entries_old_format > 0)
      {
        throw std::domain_error("Reading annotation in old format not supported. Please open an issue and supply an example file if you need this.\n");
      }
      else
      {
        int32_t colortable_format_version = -num_colortable_entries_old_format; // If the value is negative, we are in new format and its absolute value is the format version.
        if (colortable_format_version == 2)
        {
          int32_t num_colortable_entries = _freadt<int32_t>(*is); // This time for real.
          _read_annot_colortable(&annot->colortable, is, num_colortable_entries);
        }
        else
        {
          throw std::domain_error("Reading annotation in new format version !=2 not supported. Please open an issue and supply an example file if you need this.\n");
        }
      }
    }
    else
    {
      throw std::domain_error("Reading annotation without colortable not supported. Maybe invalid annotation file?\n");
    }
  }

  void read_annot(Annot *annot, const std::string &filename)
  {
    std::ifstream is(filename, std::fstream::in | std::fstream::binary);
    if (is.is_open())
    {
      read_annot(annot, &is);
      is.close();
    }
    else
    {
      throw std::runtime_error("Could not open annot file '" + filename + "' for reading.\n");
    }
  }

  std::vector<float> read_curv_data(const std::string &filename)
  {
    Curv curv;
    read_curv(&curv, filename);
    return (curv.data);
  }

  std::vector<float> read_desc_data(const std::string &filename)
  {
    if (fs::util::ends_with(filename, {".MGH", ".mgh"}))
    {
      fs::Mgh mgh;
      fs::read_mgh(&mgh, filename);
      assert(mgh.header.dtype == fs::MRI_FLOAT);
      int num_gt_1 = 0;
      std::vector<int> dims = {mgh.header.dim1length, mgh.header.dim2length, mgh.header.dim3length, mgh.header.dim4length};
      for (size_t i = 0; i < dims.size(); i++)
      {
        if (dims[i] > 1)
        {
          num_gt_1++;
        }
      }
      if (num_gt_1 > 1)
      {
        std::cerr << "MGH file '" << filename << "' contains more than one non-empty dimension. Returning concatinated data.\n";
      }
      return mgh.data.data_mri_float;
    }
    else
    {
      Curv curv;
      read_curv(&curv, filename);
      return (curv.data);
    }
  }

  template <typename T>
  T _swap_endian(T u)
  {

    static_assert(CHAR_BIT == 8, "CHAR_BIT != 8");

    union
    {
      T u;
      unsigned char u8[sizeof(T)];
    } source, dest;

    source.u = u;

    for (size_t k = 0; k < sizeof(T); k++)
      dest.u8[k] = source.u8[sizeof(T) - k - 1];

    return (dest.u);
  }

  template <typename T>
  T _freadt(std::istream &is)
  {
    T t;
    is.read(reinterpret_cast<char *>(&t), sizeof(t));
    if (!_is_bigendian())
    {
      t = _swap_endian<T>(t);
    }
    return (t);
  }

  int _fread3(std::istream &is)
  {
    uint32_t i;
    is.read(reinterpret_cast<char *>(&i), 3);
    if (!_is_bigendian())
    {
      i = _swap_endian<std::uint32_t>(i);
    }
    i = ((i >> 8) & 0xffffff);
    return (i);
  }

  template <typename T>
  void _fwritet(std::ostream &os, T t)
  {
    if (!_is_bigendian())
    {
      t = _swap_endian<T>(t);
    }
    os.write(reinterpret_cast<const char *>(&t), sizeof(t));
  }

  // Write big endian 24 bit integer to a stream, extracted from the first 3 bytes of an unsigned 32 bit integer.
  //
  // THIS FUNCTION IS INTERNAL AND SHOULD NOT BE CALLED BY API CLIENTS.
  void _fwritei3(std::ostream &os, uint32_t i)
  {
    unsigned char b1 = (i >> 16) & 255;
    unsigned char b2 = (i >> 8) & 255;
    unsigned char b3 = i & 255;

    if (!_is_bigendian())
    {
      b1 = _swap_endian<unsigned char>(b1);
      b2 = _swap_endian<unsigned char>(b2);
      b3 = _swap_endian<unsigned char>(b3);
    }

    os.write(reinterpret_cast<const char *>(&b1), sizeof(b1));
    os.write(reinterpret_cast<const char *>(&b2), sizeof(b2));
    os.write(reinterpret_cast<const char *>(&b3), sizeof(b3));
  }

  std::string _freadstringnewline(std::istream &is)
  {
    std::string s;
    std::getline(is, s, '\n');
    return s;
  }

  std::string _freadfixedlengthstring(std::istream &is, size_t length, bool strip_last_char = true)
  {
    std::string str;
    str.resize(length);
    is.read(&str[0], length);
    if (strip_last_char)
    {
      str = str.substr(0, length - 1);
    }
    return str;
  }

  void write_curv(std::ostream &os, std::vector<float> curv_data, int32_t num_faces = 100000)
  {
    const uint32_t CURV_MAGIC = 16777215;
    _fwritei3(os, CURV_MAGIC);
    _fwritet<int32_t>(os, int(curv_data.size()));
    _fwritet<int32_t>(os, num_faces);
    _fwritet<int32_t>(os, 1); // Number of values per vertex.
    for (size_t i = 0; i < curv_data.size(); i++)
    {
      _fwritet<float>(os, curv_data[i]);
    }
  }

  void write_curv(const std::string &filename, std::vector<float> curv_data, const int32_t num_faces = 100000)
  {
    std::ofstream ofs;
    ofs.open(filename, std::ofstream::out | std::ofstream::binary);
    if (ofs.is_open())
    {
      write_curv(ofs, curv_data, num_faces);
      ofs.close();
    }
    else
    {
      throw std::runtime_error("Unable to open curvature file '" + filename + "' for writing.\n");
    }
  }

  void write_mgh(const Mgh &mgh, std::ostream &os)
  {
    _fwritet<int32_t>(os, 1); // MGH file format version
    _fwritet<int32_t>(os, mgh.header.dim1length);
    _fwritet<int32_t>(os, mgh.header.dim2length);
    _fwritet<int32_t>(os, mgh.header.dim3length);
    _fwritet<int32_t>(os, mgh.header.dim4length);

    _fwritet<int32_t>(os, mgh.header.dtype);
    _fwritet<int32_t>(os, mgh.header.dof);

    size_t unused_header_space_size_left = 256; // in bytes
    _fwritet<int16_t>(os, mgh.header.ras_good_flag);
    unused_header_space_size_left -= 2; // for RAS flag

    // Write RAS part of of header if flag is 1.
    if (mgh.header.ras_good_flag == 1)
    {
      _fwritet<float>(os, mgh.header.xsize);
      _fwritet<float>(os, mgh.header.ysize);
      _fwritet<float>(os, mgh.header.zsize);

      for (int i = 0; i < 9; i++)
      {
        _fwritet<float>(os, mgh.header.Mdc[i]);
      }
      for (int i = 0; i < 3; i++)
      {
        _fwritet<float>(os, mgh.header.Pxyz_c[i]);
      }

      unused_header_space_size_left -= 60;
    }

    for (size_t i = 0; i < unused_header_space_size_left; i++)
    { // Fill rest of header space.
      _fwritet<uint8_t>(os, 0);
    }

    // Write data
    size_t num_values = mgh.header.num_values();
    if (mgh.header.dtype == MRI_INT)
    {
      if (mgh.data.data_mri_int.size() != num_values)
      {
        throw std::logic_error("Detected mismatch of MRI_INT data size and MGH header dim length values.\n");
      }
      for (size_t i = 0; i < num_values; i++)
      {
        _fwritet<int32_t>(os, mgh.data.data_mri_int[i]);
      }
    }
    else if (mgh.header.dtype == MRI_FLOAT)
    {
      if (mgh.data.data_mri_float.size() != num_values)
      {
        throw std::logic_error("Detected mismatch of MRI_FLOAT data size and MGH header dim length values.\n");
      }
      for (size_t i = 0; i < num_values; i++)
      {
        _fwritet<float>(os, mgh.data.data_mri_float[i]);
      }
    }
    else if (mgh.header.dtype == MRI_UCHAR)
    {
      if (mgh.data.data_mri_uchar.size() != num_values)
      {
        throw std::logic_error("Detected mismatch of MRI_UCHAR data size and MGH header dim length values.\n");
      }
      for (size_t i = 0; i < num_values; i++)
      {
        _fwritet<uint8_t>(os, mgh.data.data_mri_uchar[i]);
      }
    }
    else if (mgh.header.dtype == MRI_SHORT)
    {
      if (mgh.data.data_mri_short.size() != num_values)
      {
        throw std::logic_error("Detected mismatch of MRI_SHORT data size and MGH header dim length values.\n");
      }
      for (size_t i = 0; i < num_values; i++)
      {
        _fwritet<short>(os, mgh.data.data_mri_short[i]);
      }
    }
    else
    {
      throw std::domain_error("Unsupported MRI data type " + std::to_string(mgh.header.dtype) + ", cannot write MGH data.\n");
    }
  }

  void write_mgh(const Mgh &mgh, const std::string &filename)
  {
    std::ofstream ofs;
    ofs.open(filename, std::ofstream::out | std::ofstream::binary);
    if (ofs.is_open())
    {
      write_mgh(mgh, ofs);
      ofs.close();
    }
    else
    {
      throw std::runtime_error("Unable to open MGH file '" + filename + "' for writing.\n");
    }
  }

  struct Label
  {

    Label() {}

    Label(std::vector<int> vertices, std::vector<float> values)
    {
      assert(vertices.size() == values.size());
      vertex = vertices;
      value = values;
      coord_x = std::vector<float>(vertices.size(), 0.0f);
      coord_y = std::vector<float>(vertices.size(), 0.0f);
      coord_z = std::vector<float>(vertices.size(), 0.0f);
    }

    Label(std::vector<int> vertices)
    {
      vertex = vertices;
      value = std::vector<float>(vertices.size(), 0.0f);
      coord_x = std::vector<float>(vertices.size(), 0.0f);
      coord_y = std::vector<float>(vertices.size(), 0.0f);
      coord_z = std::vector<float>(vertices.size(), 0.0f);
    }

    std::vector<int> vertex;    
    std::vector<float> coord_x; 
    std::vector<float> coord_y; 
    std::vector<float> coord_z; 
    std::vector<float> value;   

    std::vector<bool> vert_in_label(size_t surface_num_verts) const
    {
      if (surface_num_verts < this->vertex.size())
      { // nonsense, so we warn (but don't throw, maybe the user really wants this).
        std::cerr << "Invalid number of vertices for surface, must be at least " << this->vertex.size() << "\n";
      }
      std::vector<bool> is_in = std::vector<bool>(surface_num_verts, false);

      for (size_t i = 0; i < this->vertex.size(); i++)
      {
        is_in[this->vertex[i]] = true;
      }
      return (is_in);
    }

    size_t num_entries() const
    {
      size_t num_ent = this->vertex.size();
      if (this->coord_x.size() != num_ent || this->coord_y.size() != num_ent || this->coord_z.size() != num_ent || this->value.size() != num_ent)
      {
        std::cerr << "Inconsistent label: sizes of property vectors do not match.\n";
      }
      return (num_ent);
    }
  };

  void write_surf(std::vector<float> vertices, std::vector<int32_t> faces, std::ostream &os)
  {
    const uint32_t SURF_TRIS_MAGIC = 16777214;
    _fwritei3(os, SURF_TRIS_MAGIC);
    std::string created_and_comment_lines = "Created by fslib\n\n";
    os << created_and_comment_lines;
    _fwritet<int32_t>(os, int(vertices.size() / 3)); // number of vertices
    _fwritet<int32_t>(os, int(faces.size() / 3));    // number of faces
    for (size_t i = 0; i < vertices.size(); i++)
    {
      _fwritet<float>(os, vertices[i]);
    }
    for (size_t i = 0; i < faces.size(); i++)
    {
      _fwritet<int32_t>(os, faces[i]);
    }
  }

  void write_surf(std::vector<float> vertices, std::vector<int32_t> faces, const std::string &filename)
  {
    std::ofstream ofs;
    ofs.open(filename, std::ofstream::out | std::ofstream::binary);
    if (ofs.is_open())
    {
      write_surf(vertices, faces, ofs);
      ofs.close();
    }
    else
    {
      throw std::runtime_error("Unable to open surf file '" + filename + "' for writing.\n");
    }
  }

  void write_surf(const Mesh &mesh, const std::string &filename)
  {
    std::ofstream ofs;
    ofs.open(filename, std::ofstream::out | std::ofstream::binary);
    if (ofs.is_open())
    {
      write_surf(mesh.vertices, mesh.faces, ofs);
      ofs.close();
    }
    else
    {
      throw std::runtime_error("Unable to open surf file '" + filename + "' for writing.\n");
    }
  }

  void read_label(Label *label, std::istream *is)
  {
    std::string line;
    int line_idx = -1;
    size_t num_entries_header = 0; // number of vertices/voxels according to header
    size_t num_entries = 0;        // number of vertices/voxels for which the file contains label entries.
    while (std::getline(*is, line))
    {
      line_idx += 1;
      std::istringstream iss(line);
      if (line_idx == 0)
      {
        continue; // skip comment.
      }
      else
      {
        if (line_idx == 1)
        {
          if (!(iss >> num_entries_header))
          {
            throw std::domain_error("Could not parse entry count from label file, invalid format.\n");
          }
        }
        else
        {
          int vertex;
          float x, y, z, value;
          if (!(iss >> vertex >> x >> y >> z >> value))
          {
            throw std::domain_error("Could not parse line " + std::to_string(line_idx + 1) + " of label file, invalid format.\n");
          }
          label->vertex.push_back(vertex);
          label->coord_x.push_back(x);
          label->coord_y.push_back(y);
          label->coord_z.push_back(z);
          label->value.push_back(value);
          num_entries++;
        }
      }
    }
    if (num_entries != num_entries_header)
    {
      throw std::domain_error("Expected " + std::to_string(num_entries_header) + " entries from label file header, but found " + std::to_string(num_entries) + " in file, invalid label file.\n");
    }
    if (label->vertex.size() != num_entries || label->coord_x.size() != num_entries || label->coord_y.size() != num_entries || label->coord_z.size() != num_entries || label->value.size() != num_entries)
    {
      throw std::domain_error("Expected " + std::to_string(num_entries) + " entries in all Label vectors, but some did not match.\n");
    }
  }

  void read_label(Label *label, const std::string &filename)
  {
    std::ifstream infile(filename, std::fstream::in);
    if (infile.is_open())
    {
      read_label(label, &infile);
      infile.close();
    }
    else
    {
      throw std::runtime_error("Could not open label file '" + filename + "' for reading.\n");
    }
  }

  void write_label(const Label &label, std::ostream &os)
  {
    const size_t num_entries = label.num_entries();
    os << "#!ascii label from subject anonymous\n"
       << num_entries << "\n";
    for (size_t i = 0; i < num_entries; i++)
    {
      os << label.vertex[i] << " " << label.coord_x[i] << " " << label.coord_y[i] << " " << label.coord_z[i] << " " << label.value[i] << "\n";
    }
  }

  void write_label(const Label &label, const std::string &filename)
  {
    std::ofstream ofs;
    ofs.open(filename, std::ofstream::out);
    if (ofs.is_open())
    {
      write_label(label, ofs);
      ofs.close();
    }
    else
    {
      throw std::runtime_error("Unable to open label file '" + filename + "' for writing.\n");
    }
  }

  void write_mesh(const Mesh &mesh, const std::string &filename)
  {
    if (fs::util::ends_with(filename, {".ply", ".PLY"}))
    {
      mesh.to_ply_file(filename);
    }
    else if (fs::util::ends_with(filename, {".obj", ".OBJ"}))
    {
      mesh.to_obj_file(filename);
    }
    else if (fs::util::ends_with(filename, {".off", ".OFF"}))
    {
      mesh.to_off_file(filename);
    }
    else
    {
      fs::write_surf(mesh, filename);
    }
  }

} // End namespace fs