// Copyright 2021 DeepMind Technologies Limited
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include <algorithm>
#include <climits>
#include <cmath>
#include <cstddef>
#include <cstdio>
#include <cstring>
#include <iostream>
#include <sstream>
#include <stdexcept>
#include <string>
#include <vector>

#include <mujoco/mjmacro.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjtnum.h>
#include <mujoco/mjplugin.h>
#include "cc/array_safety.h"
#include "engine/engine_crossplatform.h"
#include "engine/engine_util_errmem.h"
#include "user/user_flexcomp.h"
#include <mujoco/mjspec.h>
#include "user/user_api.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_resource.h"
#include "user/user_util.h"

namespace {
namespace mju = ::mujoco::util;
using std::vector;
using std::stringstream;
}  // namespace

// strncpy with 0, return false
static bool comperr(char* error, const char* msg, int error_sz) {
  mju_strncpy(error, msg, error_sz);
  return false;
}

// Read data of type T from a potentially unaligned buffer pointer.
template <typename T>
static void ReadFromBuffer(T* dst, const char* src) {
  std::memcpy(dst, src, sizeof(T));
}


static void ReadStrFromBuffer(char* dest, const char* src, int maxlen) {
  std::strncpy(dest, src, maxlen);
}

bool IsValidElementOrNodeHeader22(const std::string& line) {
  // making sure characters are numbers
  for (char c : line) {
    if (!std::isdigit(c)) {
      return false;
    }
  }
  return true;
}


// constructor: set defaults outside mjCDef
mjCFlexcomp::mjCFlexcomp(void) {
  type = mjFCOMPTYPE_GRID;
  count[0] = count[1] = count[2] = 10;
  cellcount[0] = cellcount[1] = cellcount[2] = -1;
  mjuu_setvec(spacing, 0.02, 0.02, 0.02);
  mjuu_setvec(scale, 1, 1, 1);
  mass = 1;
  inertiabox = 0.005;
  equality = 0;
  mjuu_setvec(pos, 0, 0, 0);
  mjuu_setvec(quat, 1, 0, 0, 0);
  rigid = false;
  centered = false;
  doftype = mjFCOMPDOF_FULL;

  mjs_defaultPlugin(&plugin);
  mjs_defaultOrientation(&alt);
  plugin_name = "";
  plugin_instance_name = "";
  plugin.plugin_name = (mjString*)&plugin_name;
  plugin.name = (mjString*)&plugin_instance_name;
}


// identify empty cells and pin nodes exclusively in empty cells
void mjCFlexcomp::MarkEmptyCells(mjCFlex* flex, const double* points,
                                 int npnt, const double minmax[6],
                                 int nx, int ny, int nz) {
  int cx = flex->spec.cellcount[0];
  int cy = flex->spec.cellcount[1];
  int cz = flex->spec.cellcount[2];
  int ncells = cx * cy * cz;
  int order = flex->spec.order;

  // determine which cells contain mesh vertices
  flex->cell_empty.assign(ncells, true);
  for (int i = 0; i < npnt; i++) {
    // compute parametric coordinates of mesh vertex in [0, 1]^3
    // for flat meshes (zero extent along an axis), default to 0.5
    double dx = minmax[3] - minmax[0];
    double dy = minmax[4] - minmax[1];
    double dz = minmax[5] - minmax[2];
    double sx = dx > 0 ? (points[3*i+0] - minmax[0]) / dx : 0.5;
    double sy = dy > 0 ? (points[3*i+1] - minmax[1]) / dy : 0.5;
    double sz = dz > 0 ? (points[3*i+2] - minmax[2]) / dz : 0.5;

    // find containing cell
    int ci = std::min((int)(sx * cx), cx - 1);
    int cj = std::min((int)(sy * cy), cy - 1);
    int ck = std::min((int)(sz * cz), cz - 1);
    ci = std::max(ci, 0);
    cj = std::max(cj, 0);
    ck = std::max(ck, 0);

    flex->cell_empty[ci * cy * cz + cj * cz + ck] = false;
  }

  // pin nodes that belong exclusively to empty cells
  for (int gi = 0; gi < nx; gi++) {
    for (int gj = 0; gj < ny; gj++) {
      for (int gk = 0; gk < nz; gk++) {
        // find all cells that reference this node
        bool all_empty = true;
        int ci_min = std::max(0, gi == 0 ? 0 : (gi - 1) / order);
        int ci_max = std::min(cx - 1, gi / order);
        int cj_min = std::max(0, gj == 0 ? 0 : (gj - 1) / order);
        int cj_max = std::min(cy - 1, gj / order);
        int ck_min = std::max(0, gk == 0 ? 0 : (gk - 1) / order);
        int ck_max = std::min(cz - 1, gk / order);

        for (int ci = ci_min; ci <= ci_max && all_empty; ci++) {
          for (int cj = cj_min; cj <= cj_max && all_empty; cj++) {
            for (int ck = ck_min; ck <= ck_max && all_empty; ck++) {
              if (!flex->cell_empty[ci * cy * cz + cj * cz + ck]) {
                all_empty = false;
              }
            }
          }
        }

        if (all_empty) {
          int idx = gi * ny * nz + gj * nz + gk;
          pinned[idx] = true;
        }
      }
    }
  }
}


// make flexcomp object
bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz, const mjVFS* vfs) {
  mjCModel* model = static_cast<mjCBody*>(body->element)->model;
  mjsCompiler* compiler = static_cast<mjCBody*>(body->element)->compiler;
  mjsFlex* dflex = def.spec.flex;
  bool direct = (type == mjFCOMPTYPE_DIRECT ||
                 type == mjFCOMPTYPE_MESH ||
                 type == mjFCOMPTYPE_GMSH);

  // check parent body name
  if (mjs_getName(body->element)->empty()) {
    return comperr(error, "Parent body must have name", error_sz);
  }

  // check dim
  if (dflex->dim < 1 || dflex->dim > 3) {
    return comperr(error, "Invalid dim, must be between 1 and 3", error_sz);
  }

  // check counts
  for (int i=0; i < 3; i++) {
    if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) {
      return comperr(error, "Count too small", error_sz);
    }
  }

  // check spacing
  double minspace = 2*dflex->radius + dflex->margin;
  if (!direct) {
    if (spacing[0] < minspace ||
        spacing[1] < minspace ||
        spacing[2] < minspace) {
      return comperr(error, "Spacing must be larger than geometry size", error_sz);
    }
  }

  // check scale
  if (scale[0] < mjMINVAL || scale[1] < mjMINVAL || scale[2] < mjMINVAL) {
    return comperr(error, "Scale must be larger than mjMINVAL", error_sz);
  }

  // check mass and inertia
  if (mass < mjMINVAL || inertiabox < mjMINVAL) {
    return comperr(error, "Mass and inertiabox must be larger than mjMINVAL", error_sz);
  }

  // compute orientation
  const char* alterr = mjs_resolveOrientation(quat, compiler->degree, compiler->eulerseq, &alt);
  if (alterr) {
    return comperr(error, alterr, error_sz);
  }

  // type-specific constructor: populate point and element, possibly set dim
  bool res;
  switch (type) {
    case mjFCOMPTYPE_GRID:
    case mjFCOMPTYPE_CIRCLE:
      res = MakeGrid(error, error_sz);
      break;

    case mjFCOMPTYPE_BOX:
    case mjFCOMPTYPE_CYLINDER:
    case mjFCOMPTYPE_ELLIPSOID:
      res = MakeBox(error, error_sz, dflex->dim);
      break;

    case mjFCOMPTYPE_SQUARE:
    case mjFCOMPTYPE_DISC:
      res = MakeSquare(error, error_sz);
      break;

    case mjFCOMPTYPE_MESH:
      res = MakeMesh(model, compiler, error, error_sz, vfs);
      break;

    case mjFCOMPTYPE_GMSH:
      res = MakeGMSH(model, compiler, error, error_sz, vfs);
      break;

    case mjFCOMPTYPE_DIRECT:
      res = true;
      break;

    default:
      return comperr(error, "Unknown flexcomp type", error_sz);
  }
  if (!res) {
    return false;
  }

  // force flatskin shading for box, cylinder and 3D grid
  if (type == mjFCOMPTYPE_BOX || type == mjFCOMPTYPE_CYLINDER ||
      (type == mjFCOMPTYPE_GRID && dflex->dim == 3)) {
    dflex->flatskin = true;
  }

  // check pin sizes
  if (pinrange.size()%2) {
    return comperr(error, "Pin range number must be multiple of 2", error_sz);
  }
  if (pingrid.size()%dflex->dim) {
    return comperr(error, "Pin grid number must be multiple of dim", error_sz);
  }
  if (pingridrange.size()%(2*dflex->dim)) {
    return comperr(error, "Pin grid range number of must be multiple of 2*dim", error_sz);
  }
  if (type != mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty()) &&
      doftype != mjFCOMPDOF_TRILINEAR && doftype != mjFCOMPDOF_QUADRATIC) {
    return comperr(error, "Pin grid(range) can only be used with grid or interpolated", error_sz);
  }
  if (dflex->dim == 1 && !(pingrid.empty() && pingridrange.empty())) {
    return comperr(error, "Pin grid(range) cannot be used with dim=1", error_sz);
  }

  // require element and point
  if (point.empty() || element.empty()) {
    return comperr(error, "Point and element required", error_sz);
  }

  // check point size
  if (point.size()%3) {
    return comperr(error, "Point size must be a multiple of 3", error_sz);
  }

  // check element size
  if (element.size()%(dflex->dim+1)) {
    return comperr(error, "Element size must be a multiple of dim+1", error_sz);
  }

  // get number of points
  int npnt = point.size()/3;

  // check elem vertex ids
  for (int i=0; i < (int)element.size(); i++) {
    if (element[i] < 0 || element[i] >= npnt) {
      char msg[100];
      snprintf(msg, sizeof(msg), "element %d has point id %d, number of points is %d", i,
               element[i], npnt);
      return comperr(error, msg, error_sz);
    }
  }

  // apply scaling for direct types
  if (direct && (scale[0] != 1 || scale[1] != 1 || scale[2] != 1)) {
    for (int i=0; i < npnt; i++) {
      point[3*i] *= scale[0];
      point[3*i+1] *= scale[1];
      point[3*i+2] *= scale[2];
    }
  }

  // apply pose transform to points
  for (int i=0; i < npnt; i++) {
    double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]};
    mjuu_trnVecPose(newp, pos, quat, oldp);
    point[3*i] = newp[0];
    point[3*i+1] = newp[1];
    point[3*i+2] = newp[2];
  }

  // compute bounding box of points
  double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
  for (int i=0; i < npnt; i++) {
    for (int j=0; j < 3; j++) {
      minmax[j+0] = std::min(minmax[j+0], point[3*i+j]);
      minmax[j+3] = std::max(minmax[j+3], point[3*i+j]);
    }
  }

  // construct pinned array
  int nnode = 0;
  if (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) {
    int order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2;
    // multi-cell count for mesh/direct/gmsh, else single cell
    int cx = 1, cy = 1, cz = 1;
    if (type == mjFCOMPTYPE_MESH || type == mjFCOMPTYPE_DIRECT ||
        type == mjFCOMPTYPE_GMSH) {
      if (cellcount[0] >= 0) {
        cx = cellcount[0];
        cy = cellcount[1];
        cz = cellcount[2];
      }
    }
    nnode = (cx*order+1) * (cy*order+1) * (cz*order+1);
  }
  pinned = vector<bool>(std::max(npnt, nnode), rigid);

  // handle pins if user did not specify rigid
  if (!rigid) {
    // process pinid
    for (int i=0; i < (int)pinid.size(); i++) {
      // check range
      if (pinid[i] < 0 || pinid[i] >= npnt) {
        return comperr(error, "pinid out of range", error_sz);
      }

      // set
      pinned[pinid[i]] = true;
    }

    // process pinrange
    for (int i=0; i < (int)pinrange.size(); i+=2) {
      // check range
      if (pinrange[i] < 0 || pinrange[i] >= npnt ||
          pinrange[i+1] < 0 || pinrange[i+1] >= npnt) {
        return comperr(error, "pinrange out of range", error_sz);
      }

      // set
      for (int k=pinrange[i]; k <= pinrange[i+1]; k++) {
        pinned[k] = true;
      }
    }

    // process pingrid
    for (int i=0; i < (int)pingrid.size(); i+=dflex->dim) {
      // check range
      int count_check[3] = {count[0], count[1], count[2]};
      if (type != mjFCOMPTYPE_GRID && (doftype == mjFCOMPDOF_TRILINEAR ||
                                       doftype == mjFCOMPDOF_QUADRATIC)) {
        int dim = (doftype == mjFCOMPDOF_TRILINEAR) ? 2 : 3;
        count_check[0] = count_check[1] = count_check[2] = dim;
      }
      for (int k=0; k < dflex->dim; k++) {
        if (pingrid[i+k] < 0 || pingrid[i+k] >= count_check[k]) {
          return comperr(error, "pingrid out of range", error_sz);
        }
      }

      // set
      if (dflex->dim == 2) {
        pinned[GridID(pingrid[i], pingrid[i+1])] = true;
      }
      else if (dflex->dim == 3) {
        pinned[GridID(pingrid[i], pingrid[i+1], pingrid[i+2])] = true;
      }
    }

    // process pingridrange
    for (int i=0; i < (int)pingridrange.size(); i+=2*dflex->dim) {
      // check range
      for (int k=0; k < 2*dflex->dim; k++) {
        if (pingridrange[i+k] < 0 || pingridrange[i+k] >= count[k%dflex->dim]) {
          return comperr(error, "pingridrange out of range", error_sz);
        }
      }

      // set
      if (dflex->dim == 2) {
        for (int ix=pingridrange[i]; ix <= pingridrange[i+2]; ix++) {
          for (int iy=pingridrange[i+1]; iy <= pingridrange[i+3]; iy++) {
            pinned[GridID(ix, iy)] = true;
          }
        }
      }
      else if (dflex->dim == 3) {
        for (int ix=pingridrange[i]; ix <= pingridrange[i+3]; ix++) {
          for (int iy=pingridrange[i+1]; iy <= pingridrange[i+4]; iy++) {
            for (int iz=pingridrange[i+2]; iz <= pingridrange[i+5]; iz++) {
              pinned[GridID(ix, iy, iz)] = true;
            }
          }
        }
      }
    }

    // center of radial body is always pinned
    if (doftype == mjFCOMPDOF_RADIAL) {
      pinned[0] = true;
    }

    // check if all or none are pinned
    bool allpin = true, nopin = true;
    for (int i=0; i < npnt; i++) {
      if (pinned[i]) {
        nopin = false;
      }
      else {
        allpin = false;
      }
    }

    // adjust rigid and centered
    if (allpin) {
      rigid = true;
    }
    else if (nopin) {
      centered = true;
    }
  }

  // remove unreferenced for direct, mesh, gmsh
  if (direct) {
    // find used
    used = std::vector<bool> (npnt, false);
    for (int i=0; i < (int)element.size(); i++) {
      used[element[i]] = true;
    }

    // construct reindex
    bool hasunused = false;
    std::vector<int> reindex (npnt, 0);
    for (int i=0; i < npnt; i++) {
      if (!used[i]) {
        hasunused = true;
        for (int k=i+1; k < npnt; k++) {
          reindex[k]--;
        }
      }
    }

    // reindex elements if unused present
    if (hasunused) {
      for (int i=0; i < (int)element.size(); i++) {
        element[i] += reindex[element[i]];
      }

      // compact point, texcoord, pinned arrays
      int new_npnt = 0;
      for (int i=0; i < npnt; i++) {
        if (used[i]) {
          point[3*new_npnt+0] = point[3*i+0];
          point[3*new_npnt+1] = point[3*i+1];
          point[3*new_npnt+2] = point[3*i+2];

          if (!texcoord.empty()) {
            texcoord[2*new_npnt+0] = texcoord[2*i+0];
            texcoord[2*new_npnt+1] = texcoord[2*i+1];
          }

          pinned[new_npnt] = pinned[i];
          new_npnt++;
        }
      }

      // resize arrays
      point.resize(3*new_npnt);
      if (!texcoord.empty()) {
        texcoord.resize(2*new_npnt);
      }
      pinned.resize(std::max(new_npnt, nnode));
      used.assign(new_npnt, true);

      // update count
      npnt = new_npnt;
    }
  }

  // nothing to remove for auto-generated types
  else {
    used = std::vector<bool> (npnt, true);
  }

  // create flex, copy parameters
  mjCFlex* flex = model->AddFlex();
  mjsFlex* pf = &flex->spec;
  int id = flex->id;

  *flex = def.Flex();
  flex->PointToLocal();

  flex->model = model;
  flex->id = id;
  mjs_setName(pf->element, name.c_str());
  mjs_setInt(pf->elem, element.data(), element.size());
  mjs_setFloat(pf->texcoord, texcoord.data(), texcoord.size());
  mjs_setInt(pf->elemtexcoord, elemtexcoord.data(), elemtexcoord.size());
  if (!centered) {
    mjs_setDouble(pf->vert, point.data(), point.size());
  }

  // rigid: set parent name, nothing else to do
  if (rigid) {
    mjs_appendString(pf->vertbody, mjs_getName(body->element)->c_str());
    return true;
  }

  // compute body mass and inertia matching specs
  double bodymass = mass/npnt;
  double bodyinertia = bodymass*(2.0*inertiabox*inertiabox)/3.0;

  // overwrite plugin name
  if (plugin.active && plugin_instance_name.empty()) {
    plugin_instance_name = "flexcomp_" + name;
    static_cast<mjCPlugin*>(plugin.element)->name = plugin_instance_name;
  }

  // create bodies, construct flex vert and vertbody
  for (int i=0; i < npnt; i++) {
    // not used: skip
    if (!used[i]) {
      continue;
    }

    // pinned or trilinear or quadratic: parent body
    if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) {
      mjs_appendString(pf->vertbody, mjs_getName(body->element)->c_str());

      // add plugin
      if (plugin.active) {
        mjsPlugin* pplugin = &body->plugin;
        pplugin->active = true;
        pplugin->element = static_cast<mjsElement*>(plugin.element);
        mjs_setString(pplugin->plugin_name, mjs_getString(plugin.plugin_name));
        mjs_setString(pplugin->name, plugin_instance_name.c_str());
      }
    }

    // not pinned and not trilinear: new body
    else {
      // add new body at vertex coordinates
      mjsBody* pb = mjs_addBody(body, 0);

      // set frame and inertial
      pb->pos[0] = point[3*i];
      pb->pos[1] = point[3*i+1];
      pb->pos[2] = point[3*i+2];
      mjuu_zerovec(pb->ipos, 3);
      pb->mass = bodymass;
      pb->inertia[0] = bodyinertia;
      pb->inertia[1] = bodyinertia;
      pb->inertia[2] = bodyinertia;
      pb->explicitinertial = true;

      // add radial slider
      if (doftype == mjFCOMPDOF_RADIAL) {
        mjsJoint* jnt = mjs_addJoint(pb, 0);

        // set properties
        jnt->type = mjJNT_SLIDE;
        mjuu_setvec(jnt->pos, 0, 0, 0);
        mjuu_copyvec(jnt->axis, pb->pos, 3);
        mjuu_normvec(jnt->axis, 3);
      }

      // add three orthogonal sliders
      else if (doftype == mjFCOMPDOF_FULL) {
        for (int j=0; j < 3; j++) {
          // add joint to body
          mjsJoint* jnt = mjs_addJoint(pb, 0);

          // set properties
          jnt->type = mjJNT_SLIDE;
          mjuu_setvec(jnt->pos, 0, 0, 0);
          mjuu_setvec(jnt->axis, 0, 0, 0);
          jnt->axis[j] = 1;
        }
      }

      // construct body name, add to vertbody
      char txt[100];
      mju::sprintf_arr(txt, "%s_%d", name.c_str(), i);
      mjs_setName(pb->element, txt);
      mjs_appendString(pf->vertbody, mjs_getName(pb->element)->c_str());

      // clear flex vertex coordinates if allocated
      if (!centered) {
        point[3*i] = 0;
        point[3*i+1] = 0;
        point[3*i+2] = 0;
      }

      // add plugin
      if (plugin.active) {
        mjsPlugin* pplugin = &pb->plugin;
        pplugin->active = true;
        pplugin->element = static_cast<mjsElement*>(plugin.element);
        mjs_setString(pplugin->plugin_name, mjs_getString(plugin.plugin_name));
        mjs_setString(pplugin->name, plugin_instance_name.c_str());
      }
    }
  }

  // create nodal mesh for trilinear/quadratic interpolation
  if (doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) {
    flex->spec.order = doftype == mjFCOMPDOF_TRILINEAR ? 1 : 2;

    if (cellcount[0] >= 0) {
      flex->spec.cellcount[0] = cellcount[0];
      flex->spec.cellcount[1] = cellcount[1];
      flex->spec.cellcount[2] = cellcount[2];
    }

    // total number of nodes with shared boundaries
    int nx = flex->spec.cellcount[0] * flex->spec.order + 1;
    int ny = flex->spec.cellcount[1] * flex->spec.order + 1;
    int nz = flex->spec.cellcount[2] * flex->spec.order + 1;
    int nnode = nx * ny * nz;

    // mark empty cells and pin nodes exclusively in empty cells
    MarkEmptyCells(flex, point.data(), npnt, minmax, nx, ny, nz);

    // if MarkEmptyCells pinned any nodes, force centered=false
    // so that pf->node (local positions) is saved to the model
    if (centered) {
      for (int i = 0; i < nnode; i++) {
        if (pinned[i]) {
          centered = false;
          break;
        }
      }
    }

    std::vector<double> node(3 * nnode, 0);
    int idx = 0;

    // Simpson's rule weights for quadratic mass distribution
    double massP2[3] = {1. / 6., 2. / 3., 1. / 6.};



    // collect created bodies for mass normalization
    std::vector<mjsBody*> node_bodies;

    for (int gi = 0; gi < nx; gi++) {
      for (int gj = 0; gj < ny; gj++) {
        for (int gk = 0; gk < nz; gk++) {
          // parametric position in [0, 1]^3
          double s = (double)gi / (flex->spec.cellcount[0] * flex->spec.order);
          double t = (double)gj / (flex->spec.cellcount[1] * flex->spec.order);
          double u = (double)gk / (flex->spec.cellcount[2] * flex->spec.order);

          // physical position
          double px = minmax[0] + s * (minmax[3] - minmax[0]);
          double py = minmax[1] + t * (minmax[4] - minmax[1]);
          double pz = minmax[2] + u * (minmax[5] - minmax[2]);

          if (pinned[idx]) {
            node[3*idx+0] = px;
            node[3*idx+1] = py;
            node[3*idx+2] = pz;
            mjs_appendString(pf->nodebody,
                             mjs_getName(body->element)->c_str());
            idx++;
            continue;
          }

          mjsBody* pb = mjs_addBody(body, 0);
          pb->pos[0] = px;
          pb->pos[1] = py;
          pb->pos[2] = pz;
          mjuu_zerovec(pb->ipos, 3);

          // mass distribution
          if (doftype == mjFCOMPDOF_TRILINEAR) {
            pb->mass = 1.0;
          } else {
            // local index within the cell for mass computation
            int li = gi % flex->spec.order;
            int lj = gj % flex->spec.order;
            int lk = gk % flex->spec.order;
            // boundary nodes: average mass contribution
            int ncells_i = (gi > 0 && gi < nx-1 && li == 0) ? 2 : 1;
            int ncells_j = (gj > 0 && gj < ny-1 && lj == 0) ? 2 : 1;
            int ncells_k = (gk > 0 && gk < nz-1 && lk == 0) ? 2 : 1;
            // use Simpson weights
            double wi = massP2[li == 0 ? 0 : li];
            double wj = massP2[lj == 0 ? 0 : lj];
            double wk = massP2[lk == 0 ? 0 : lk];
            pb->mass = wi * wj * wk * ncells_i * ncells_j * ncells_k;
          }

          node_bodies.push_back(pb);

          pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
          pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
          pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
          pb->explicitinertial = true;

          for (int d=0; d < 3; d++) {
            mjsJoint* jnt = mjs_addJoint(pb, 0);
            jnt->type = mjJNT_SLIDE;
            mjuu_setvec(jnt->pos, 0, 0, 0);
            mjuu_setvec(jnt->axis, 0, 0, 0);
            jnt->axis[d] = 1;
          }

          // construct node name, add to nodebody
          char txt[100];
          mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), gi, gj, gk);
          mjs_setName(pb->element, txt);
          mjs_appendString(pf->nodebody, mjs_getName(pb->element)->c_str());

          idx++;
        }
      }
    }

    // normalize masses so total equals prescribed mass
    double total_mass = 0;
    for (mjsBody* pb : node_bodies) {
      total_mass += pb->mass;
    }
    if (total_mass > 0) {
      double scale = mass / total_mass;
      for (mjsBody* pb : node_bodies) {
        pb->mass *= scale;
        pb->inertia[0] *= scale;
        pb->inertia[1] *= scale;
        pb->inertia[2] *= scale;
      }
    }

    if (!centered) {
      mjs_setDouble(pf->node, node.data(), node.size());
    }
  }

  if (!centered || doftype == mjFCOMPDOF_TRILINEAR || doftype == mjFCOMPDOF_QUADRATIC) {
    mjs_setDouble(pf->vert, point.data(), point.size());
  }

  // create equality constraints
  if (equality) {
    // equality 1=edge(mjEQ_FLEX), 2=vert(mjEQ_FLEXVERT), 3=strain(mjEQ_FLEXSTRAIN)
    if (equality == 1 || equality == 2) {
      mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
      mjs_setDefault(pe->element, &model->Default()->spec);
      pe->type = (equality == 1) ? mjEQ_FLEX : mjEQ_FLEXVERT;
      pe->active = true;
      mjs_setString(pe->name1, name.c_str());
    } else if (equality == 3) {
      // create one strain constraint per cell, storing cell index in eq_data
      flex->has_strain_eq = true;
      int cell_cx = flex->spec.cellcount[0];
      int cell_cy = flex->spec.cellcount[1];
      int cell_cz = flex->spec.cellcount[2];
      for (int ci = 0; ci < cell_cx; ci++) {
        for (int cj = 0; cj < cell_cy; cj++) {
          for (int ck = 0; ck < cell_cz; ck++) {
            // skip empty cells
            if (!flex->cell_empty.empty() &&
                flex->cell_empty[ci * cell_cy * cell_cz + cj * cell_cz + ck]) {
              continue;
            }
            mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
            mjs_setDefault(pe->element, &model->Default()->spec);
            pe->type = mjEQ_FLEXSTRAIN;
            pe->active = true;
            mjs_setString(pe->name1, name.c_str());
            pe->data[0] = ci;
            pe->data[1] = cj;
            pe->data[2] = ck;
          }
        }
      }
    }
  }

  return true;
}



// get point id from grid coordinates
int mjCFlexcomp::GridID(int ix, int iy) {
  return ix*count[1] + iy;
}
int mjCFlexcomp::GridID(int ix, int iy, int iz) {
  return ix*count[1]*count[2] + iy*count[2] + iz;
}



// make grid
bool mjCFlexcomp::MakeGrid(char* error, int error_sz) {
  int dim = def.Flex().spec.dim;
  bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0];

  // 1D
  if (dim == 1) {
    for (int ix=0; ix < count[0]; ix++) {
      if (type == mjFCOMPTYPE_CIRCLE) {
        if (ix >= count[0]-1) {
          continue;
        }

        // add point
        double theta = 2*mjPI/(count[0]-1);
        double radius = spacing[0]/std::sin(theta/2)/2;
        point.push_back(radius*std::cos(theta*ix));
        point.push_back(radius*std::sin(theta*ix));
        point.push_back(0);

        // add element
        element.push_back(ix);
        element.push_back(ix == count[0]-2 ? 0 : ix+1);
      } else {
        // add point
        point.push_back(spacing[0]*(ix - 0.5*(count[0]-1)));
        point.push_back(0);
        point.push_back(0);

        // add element
        if (ix < count[0]-1) {
          element.push_back(ix);
          element.push_back(ix+1);
        }
      }
    }
  }

  // 2D
  else if (dim == 2) {
    for (int ix=0; ix < count[0]; ix++) {
      for (int iy=0; iy < count[1]; iy++) {
        int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}};

        // add point
        double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)),
                         spacing[1]*(iy - 0.5*(count[1]-1))};
        point.push_back(pos[0]);
        point.push_back(pos[1]);
        point.push_back(0);

        // add texture coordinates, if not specified explicitly
        if (needtex) {
          texcoord.push_back(ix/(double)std::max(count[0]-1, 1));
          texcoord.push_back(iy/(double)std::max(count[1]-1, 1));
        }

        // flip triangles if radial projection is requested
        if (((pos[0] < -mjEPS && pos[1] > -mjEPS) ||
             (pos[0] > -mjEPS && pos[1] < -mjEPS)) &&
            type == mjFCOMPTYPE_DISC) {
          quad2tri[0][2] = 3;
          quad2tri[1][0] = 1;
        }

        // add elements
        if (ix < count[0]-1 && iy < count[1]-1) {
          int vert[4] = {
            count[2]*count[1]*(ix+0) + count[2]*(iy+0),
            count[2]*count[1]*(ix+1) + count[2]*(iy+0),
            count[2]*count[1]*(ix+1) + count[2]*(iy+1),
            count[2]*count[1]*(ix+0) + count[2]*(iy+1),
          };
          for (int s =0; s < 2; s++) {
            for (int v=0; v < 3; v++) {
              element.push_back(vert[quad2tri[s][v]]);
            }
          }
        }
      }
    }
  }

  // 3D
  else {
    int cube2tets[6][4] = {{0, 3, 1, 7}, {0, 1, 4, 7},
                           {1, 3, 2, 7}, {1, 2, 6, 7},
                           {1, 5, 4, 7}, {1, 6, 5, 7}};
    for (int ix=0; ix < count[0]; ix++) {
      for (int iy=0; iy < count[1]; iy++) {
        for (int iz=0; iz < count[2]; iz++) {
          // add point
          point.push_back(spacing[0]*(ix - 0.5*(count[0]-1)));
          point.push_back(spacing[1]*(iy - 0.5*(count[1]-1)));
          point.push_back(spacing[2]*(iz - 0.5*(count[2]-1)));

          // add texture coordinates, if not specified explicitly
          if (needtex) {
            texcoord.push_back(ix/(float)std::max(count[0]-1, 1));
            texcoord.push_back(iy/(float)std::max(count[1]-1, 1));
          }

          // add elements
          if (ix < count[0]-1 && iy < count[1]-1 && iz < count[2]-1) {
            int vert[8] = {
              count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+0,
              count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+0,
              count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+0,
              count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+0,
              count[2]*count[1]*(ix+0) + count[2]*(iy+0) + iz+1,
              count[2]*count[1]*(ix+1) + count[2]*(iy+0) + iz+1,
              count[2]*count[1]*(ix+1) + count[2]*(iy+1) + iz+1,
              count[2]*count[1]*(ix+0) + count[2]*(iy+1) + iz+1,
            };
            for (int s=0; s < 6; s++) {
              for (int v=0; v < 4; v++) {
                element.push_back(vert[cube2tets[s][v]]);
              }
            }
          }
        }
      }
    }
  }

  // check elements
  if (element.empty()) {
    return comperr(error, "No elements were created in grid", error_sz);
  }

  return true;
}



// get point id from box coordinates and side
int mjCFlexcomp::BoxID(int ix, int iy, int iz) {
  // side iz=0
  if (iz == 0) {
    return ix*count[1] + iy + 1;
  }

  // side iz=max
  else if (iz == count[2]-1) {
    return count[0]*count[1] + ix*count[1] + iy + 1;
  }

  // side iy=0
  else if (iy == 0) {
    return 2*count[0]*count[1] + ix*(count[2]-2) + iz - 1 + 1;
  }

  // side iy=max
  else if (iy == count[1]-1) {
    return 2*count[0]*count[1] + count[0]*(count[2]-2) + ix*(count[2]-2) + iz - 1 + 1;
  }

  // side ix=0
  else if (ix == 0) {
    return 2*count[0]*count[1] + 2*count[0]*(count[2]-2) + (iy-1)*(count[2]-2) + iz - 1 + 1;
  }

  // side ix=max
  else {
    return 2*count[0]*count[1] + 2*count[0]*(count[2]-2) + (count[1]-2)*(count[2]-2) +
           (iy-1)*(count[2]-2) + iz - 1 + 1;
  }
}



// project from box to other shape
void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) {
  // init point
  pos[0] = 2.0*ix/(count[0]-1) - 1;
  pos[1] = 2.0*iy/(count[1]-1) - 1;
  pos[2] = 2.0*iz/(count[2]-1) - 1;

  // determine sizes
  double size[3] = {
    0.5*spacing[0]*(count[0]-1),
    0.5*spacing[1]*(count[1]-1),
    0.5*spacing[2]*(count[2]-1)
  };

  // box
  if (type == mjFCOMPTYPE_BOX) {
    pos[0] *= size[0];
    pos[1] *= size[1];
    pos[2] *= size[2];
  }

  // cylinder
  else if (type == mjFCOMPTYPE_CYLINDER) {
    double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
    mjuu_normvec(pos, 2);
    pos[0] *= size[0]*L0;
    pos[1] *= size[1]*L0;
    pos[2] *= size[2];
  }

  // ellipsoid
  else if (type == mjFCOMPTYPE_ELLIPSOID) {
    mjuu_normvec(pos, 3);
    pos[0] *= size[0];
    pos[1] *= size[1];
    pos[2] *= size[2];
  }
}



// make 2d square or disc
bool mjCFlexcomp::MakeSquare(char* error, int error_sz) {
  // set 2D
  def.spec.flex->dim = 2;

  // create square
  if (!MakeGrid(error, error_sz)) {
    return false;
  }

  // do projection
  if (type == mjFCOMPTYPE_DISC) {
    double size[2] = {
      0.5*spacing[0]*(count[0]-1),
      0.5*spacing[1]*(count[1]-1),
    };

    for (int i=0; i < point.size()/3; i++) {
      double* pos = point.data() + i*3;
      double L0 = std::max(std::abs(pos[0]), std::abs(pos[1]));
      mjuu_normvec(pos, 2);
      pos[0] *= size[0]*L0;
      pos[1] *= size[1]*L0;
    }
  }

  return true;
}



static int mat2lin(int ix, int iy, int iz, const int count[3]) {
  return ix*count[1]*count[2] + iy*count[2] + iz;
}



// make 3d box, ellipsoid or cylinder
bool mjCFlexcomp::MakeBox(char* error, int error_sz, int dim, bool open) {
  double pos[3];
  bool needtex = texcoord.empty() && mjs_getString(def.spec.flex->material)[0];

  // set dimension
  def.spec.flex->dim = dim;

  // add center point
  if (dim == 3) {
    point.push_back(0);
    point.push_back(0);
    point.push_back(0);
  }

  // add texture coordinates, if not specified explicitly
  if (needtex) {
    texcoord.push_back(0);
    texcoord.push_back(0);
  }

  // add points
  int n = 0;
  std::vector<int> idx(count[0]*count[1]*count[2]);

  // iz=0/max
  for (int iz=0; iz < count[2]; iz+=count[2]-1) {
    for (int ix=0; ix < count[0]; ix++) {
      for (int iy=0; iy < count[1]; iy++) {
        if (open && dim == 2 && iz != 0) {
          continue;
        }

        // add point
        BoxProject(pos, ix, iy, iz);
        point.push_back(pos[0]);
        point.push_back(pos[1]);
        point.push_back(pos[2]);
        idx[mat2lin(ix, iy, iz, count)] = n++;

        // add texture coordinates, if not specified explicitly
        if (needtex) {
          texcoord.push_back(ix/(float)std::max(count[0]-1, 1));
          texcoord.push_back(iy/(float)std::max(count[1]-1, 1));
        }
      }
    }
  }

  // iy=0/max
  for (int iy=0; iy < count[1]; iy+=count[1]-1) {
    for (int ix=0; ix < count[0]; ix++) {
      for (int iz=0; iz < count[2]; iz++) {
        // add point
        if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1))) {
          BoxProject(pos, ix, iy, iz);
          point.push_back(pos[0]);
          point.push_back(pos[1]);
          point.push_back(pos[2]);
          idx[mat2lin(ix, iy, iz, count)] = n++;

          // add texture coordinates
          if (needtex) {
            texcoord.push_back(ix/(float)std::max(count[0]-1, 1));
            texcoord.push_back(iz/(float)std::max(count[2]-1, 1));
          }
        }
      }
    }
  }

  // ix=0/max
  for (int ix=0; ix < count[0]; ix+=count[0]-1) {
    for (int iy=0; iy < count[1]; iy++) {
      for (int iz=0; iz < count[2]; iz++) {
        // add point
        if (iz > 0 && ((open && dim == 2) || (iz < count[2]-1)) && iy > 0 && iy < count[1]-1) {
          BoxProject(pos, ix, iy, iz);
          point.push_back(pos[0]);
          point.push_back(pos[1]);
          point.push_back(pos[2]);
          idx[mat2lin(ix, iy, iz, count)] = n++;

          // add texture coordinates
          if (needtex) {
            texcoord.push_back(iy/(float)std::max(count[1]-1, 1));
            texcoord.push_back(iz/(float)std::max(count[2]-1, 1));
          }
        }
      }
    }
  }

  // add elements

  // iz=0/max
  for (int iz=0; iz < count[2]; iz+=count[2]-1) {
    for (int ix=0; ix < count[0]; ix++) {
      for (int iy=0; iy < count[1]; iy++) {
        if (open && dim == 2 && iz != 0) {
          continue;
        }

        if (ix < count[0]-1 && iy < count[1]-1) {
          if (dim==3) {
            element.push_back(0);
            element.push_back(BoxID(ix, iy, iz));
            element.push_back(BoxID(ix+1, iy, iz));
            element.push_back(BoxID(ix+1, iy+1, iz));

            element.push_back(0);
            element.push_back(BoxID(ix, iy, iz));
            element.push_back(BoxID(ix, iy+1, iz));
            element.push_back(BoxID(ix+1, iy+1, iz));
          } else {
            int step1 = iz == 0 ? 1 : 0;
            int step2 = iz == 0 ? 0 : 1;
            element.push_back(idx[mat2lin(ix, iy, iz, count)]);
            element.push_back(idx[mat2lin(ix+1, iy+step1, iz, count)]);
            element.push_back(idx[mat2lin(ix+1, iy+step2, iz, count)]);

            element.push_back(idx[mat2lin(ix, iy, iz, count)]);
            element.push_back(idx[mat2lin(ix+step2, iy+1, iz, count)]);
            element.push_back(idx[mat2lin(ix+step1, iy+1, iz, count)]);
          }
        }
      }
    }
  }

  // iy=0/max
  for (int iy=0; iy < count[1]; iy+=count[1]-1) {
    for (int ix=0; ix < count[0]; ix++) {
      for (int iz=0; iz < count[2]; iz++) {
        if (ix < count[0]-1 && iz < count[2]-1) {
          if (dim==3) {
            element.push_back(0);
            element.push_back(BoxID(ix, iy, iz));
            element.push_back(BoxID(ix+1, iy, iz));
            element.push_back(BoxID(ix+1, iy, iz+1));

            element.push_back(0);
            element.push_back(BoxID(ix, iy, iz));
            element.push_back(BoxID(ix, iy, iz+1));
            element.push_back(BoxID(ix+1, iy, iz+1));
          } else {
            int ix0 = iy == 0 ? ix : ix+1;
            int dx = iy == 0 ? 1 : -1;
            element.push_back(idx[mat2lin(ix0, iy, iz, count)]);
            element.push_back(idx[mat2lin(ix0+dx, iy, iz, count)]);
            element.push_back(idx[mat2lin(ix0+dx, iy, iz+1, count)]);

            element.push_back(idx[mat2lin(ix0, iy, iz, count)]);
            element.push_back(idx[mat2lin(ix0+dx, iy, iz+1, count)]);
            element.push_back(idx[mat2lin(ix0, iy, iz+1, count)]);
          }
        }
      }
    }
  }

  // ix=0/max
  for (int ix=0; ix < count[0]; ix+=count[0]-1) {
    for (int iy=0; iy < count[1]; iy++) {
      for (int iz=0; iz < count[2]; iz++) {
        if (iy < count[1]-1 && iz < count[2]-1) {
          if (dim==3) {
            element.push_back(0);
            element.push_back(BoxID(ix, iy, iz));
            element.push_back(BoxID(ix, iy+1, iz));
            element.push_back(BoxID(ix, iy+1, iz+1));

            element.push_back(0);
            element.push_back(BoxID(ix, iy, iz));
            element.push_back(BoxID(ix, iy, iz+1));
            element.push_back(BoxID(ix, iy+1, iz+1));
          } else {
            int iy0 = ix != 0 ? iy : iy+1;
            int dy = ix != 0 ? 1 : -1;
            element.push_back(idx[mat2lin(ix, iy0, iz, count)]);
            element.push_back(idx[mat2lin(ix, iy0+dy, iz, count)]);
            element.push_back(idx[mat2lin(ix, iy0+dy, iz+1, count)]);

            element.push_back(idx[mat2lin(ix, iy0, iz, count)]);
            element.push_back(idx[mat2lin(ix, iy0+dy, iz+1, count)]);
            element.push_back(idx[mat2lin(ix, iy0, iz+1, count)]);

          }
        }
      }
    }
  }

  return true;
}



// copied from user_mesh.cc
template <typename T> static T* VecToArray(std::vector<T>& vector, bool clear = true){
  if (vector.empty())
    return nullptr;
  else {
    int n = (int)vector.size();
    T* cvec = (T*) mju_malloc(n*sizeof(T));
    memcpy(cvec, vector.data(), n*sizeof(T));
    if (clear) {
      vector.clear();
    }
    return cvec;
  }
}



// make mesh
bool mjCFlexcomp::MakeMesh(mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, const mjVFS* vfs) {
  // strip path
  if (!file.empty() && model->spec.strippath) {
    file = mjuu_strippath(file);
  }

  // file is required
  if (file.empty()) {
    return comperr(error, "File is required", error_sz);
  }

  // check dim
  if (def.spec.flex->dim < 2) {
    return comperr(error, "Flex dim must be at least 2 for mesh", error_sz);
  }

  // load resource
  std::string filename = mjuu_combinePaths(mjs_getString(compiler->meshdir), file);
  mjResource* resource = nullptr;


  if (mjCMesh::IsMSH(filename)) {
    return comperr(error, "legacy MSH files are not supported in flexcomp", error_sz);
  }

  try {
    resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir),
                                     filename, vfs);
  } catch (mjCError err) {
    return comperr(error, err.message, error_sz);
  }


  // load mesh
  mjCMesh mesh;
  try {
    mesh.LoadFromResource(resource, true);
    mju_closeResource(resource);
  } catch (mjCError err) {
    mju_closeResource(resource);
    return comperr(error, err.message, error_sz);
  }

  // check sizes
  if (mesh.Vert().empty() || mesh.Face().empty()) {
    return comperr(error, "Vertex and face data required", error_sz);
  }

  // copy vertices
  point.assign(mesh.Vert().begin(), mesh.Vert().end());

  if (mesh.HasTexcoord()) {
    texcoord = mesh.Texcoord();
    elemtexcoord = mesh.FaceTexcoord();
  }

  // copy faces or create 3D mesh
  if (def.spec.flex->dim == 2) {
    element = mesh.Face();
  } else {
    point.insert(point.begin() + 0, origin[0]);
    point.insert(point.begin() + 1, origin[1]);
    point.insert(point.begin() + 2, origin[2]);
    for (int i=0; i < mesh.Face().size(); i+=3) {
      // only add tetrahedra with positive volume
      int tet[3] = {mesh.Face()[i+0]+1,
                    mesh.Face()[i+1]+1,
                    mesh.Face()[i+2]+1};
      double edge1[3], edge2[3], edge3[3];
      for (int i=0; i < 3; i++) {
        edge1[i] = point[3*tet[0]+i] - origin[i];
        edge2[i] = point[3*tet[1]+i] - origin[i];
        edge3[i] = point[3*tet[2]+i] - origin[i];
      }
      double normal[3];
      mjuu_crossvec(normal, edge1, edge2);
      if (mjuu_dot3(normal, edge3) < mjMINVAL) {
        continue;
      }
      element.push_back(0);
      element.push_back(tet[0]);
      element.push_back(tet[1]);
      element.push_back(tet[2]);
    }
  }

  return true;
}



// find string in buffer, return position or -1 if not found
static int findstring(const char* buffer, int buffer_sz, const char* str) {
  int len = (int)strlen(str);

  // scan buffer
  for (int i=0; i < buffer_sz-len; i++) {
    // check for string at position i
    bool found = true;
    for (int k=0; k < len; k++) {
      if (buffer[i+k] != str[k]) {
        found = false;
        break;
      }
    }

    // string found
    if (found) {
      return i;
    }
  }

  // not found
  return -1;
}



// load points and elements from GMSH file
bool mjCFlexcomp::MakeGMSH(mjCModel* model, mjsCompiler* compiler, char* error, int error_sz, const mjVFS* vfs) {
  // strip path
  if (!file.empty() && model->spec.strippath) {
    file = mjuu_strippath(file);
  }

  // file is required
  if (file.empty()) {
    return comperr(error, "File is required", error_sz);
  }

  // open resource
  mjResource* resource = nullptr;
  try {
    std::string filename = mjuu_combinePaths(mjs_getString(compiler->meshdir), file);
    resource = mjCBase::LoadResource(mjs_getString(model->spec.modelfiledir),
                                     filename, vfs);
  } catch (mjCError err) {
    return comperr(error, err.message, error_sz);
  }

  // try to load, close resource properly
  try {
    LoadGMSH(model, resource);
    mju_closeResource(resource);
  } catch (mjCError err) {
    mju_closeResource(resource);
    return comperr(error, err.message, error_sz);
  } catch (...) {
    mju_closeResource(resource);
    return comperr(error, "exception while reading GMSH file", error_sz);
  }

  return true;
}



// load GMSH format 4.1
void mjCFlexcomp::LoadGMSH41(char* buffer, int binary, int nodeend,
                             int nodebegin, int elemend, int elembegin){
  // header size
  constexpr int kGmsh41HeaderSize = 52;
  // base for node tags, to be subtracted from element data
  size_t minNodeTag, numEntityBlocks, numNodes, maxNodeTag, numNodesInBlock, tag;
  int entityDim, entityTag, parametric;

  // ascii nodes
  if (binary == 0) {
    // convert node char buffer to stringstream
    stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin));

    // read header
    ss >> numEntityBlocks >> numNodes >> minNodeTag >> maxNodeTag;
    ss >> entityDim >> entityTag >> parametric >> numNodesInBlock;
    if (!ss.good()) {
      throw mjCError(NULL, "Error reading Nodes header");
    }

    // check number of nodes is a positive number
    if (numNodes < 0) {
      throw mjCError(NULL, "Invalid number of nodes");
    }

    // require single block
    if (numEntityBlocks != 1 || numNodes != numNodesInBlock) {
      throw mjCError(NULL, "All nodes must be in single block");
    }

    // require maximum number of nodes be equal to maximum number of nodes in a block
    if (maxNodeTag != numNodesInBlock){
      throw mjCError(NULL, "Maximum number of nodes must be equal to number of nodes in a block");
    }

    // check dimensionality and save
    if (entityDim < 1 || entityDim > 3) {
      throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
    }
    def.spec.flex->dim = entityDim;

    // read and discard node tags; require range from minNodeTag to maxNodeTag
    for (size_t i=0; i < numNodes; i++) {
      size_t tag;
      ss >> tag;
      if (!ss.good()) {
        throw mjCError(NULL, "Error reading node tags");
      }
      if (tag != i+minNodeTag) {
        throw mjCError(NULL, "Node tags must be sequential");
      }
    }

    // read points
    if (numNodes < 0 || numNodes >= INT_MAX / 3) {
      throw mjCError(NULL, "Invalid number of nodes.");
    }
    point.reserve(3*numNodes);
    for (size_t i=0; i < 3*numNodes; i++) {
      double x;
      ss >> x;
      if (!ss.good()) {
        throw mjCError(NULL, "Error reading node coordinates");
      }
      point.push_back(x);
    }
  }
  // binary nodes
  else {
    // check header size
    if (nodeend-nodebegin < kGmsh41HeaderSize) {
      throw mjCError(NULL, "Invalid nodes header");
    }

    // read header
    ReadFromBuffer(&numEntityBlocks, buffer + nodebegin);
    ReadFromBuffer(&numNodes, buffer + nodebegin + 8);
    ReadFromBuffer(&minNodeTag, buffer + nodebegin + 16);
    ReadFromBuffer(&maxNodeTag, buffer + nodebegin + 24);
    ReadFromBuffer(&entityDim, buffer + nodebegin + 32);
    ReadFromBuffer(&entityTag, buffer + nodebegin + 36);
    ReadFromBuffer(&parametric, buffer + nodebegin + 40);
    ReadFromBuffer(&numNodesInBlock, buffer + nodebegin + 44);

    // require single block
    if (numEntityBlocks != 1 || numNodes != numNodesInBlock) {
      throw mjCError(NULL, "All nodes must be in single block");
    }

    // check number of nodes is a positive number
    if (numNodes < 0) {
      throw mjCError(NULL, "Invalid number of nodes");
    }

    // check dimensionality and save
    if (entityDim < 1 || entityDim > 3) {
      throw mjCError(NULL, "Entity must be 1D, 2D or 3D");
    }
    def.spec.flex->dim = entityDim;

    // nodeData: node tag and 3 nodes
    constexpr int numNodeComponents = 4;
    constexpr int componentSize = 8;
    int nodeDataSize = numNodeComponents*componentSize;

    // check section byte size
    if (nodeend-nodebegin < kGmsh41HeaderSize + numNodes*nodeDataSize) {
      throw mjCError(NULL, "Insufficient byte size of Nodes");
    }

    // check node tags: must range from minNodeTag to maxNodeTag
    const char* tagbuffer = buffer + nodebegin + kGmsh41HeaderSize;
    for (size_t i=0; i < numNodes; i++) {
      ReadFromBuffer(&tag, tagbuffer + i*componentSize);
      if (tag != i+minNodeTag) {
        throw mjCError(NULL, "Node tags must be sequential");
      }
    }

    // read points
    if (numNodes < 0 || numNodes >= INT_MAX / 3) {
      throw mjCError(NULL, "Invalid number of nodes.");
    }
    point.reserve(3*numNodes);
    const char* pointbuffer = buffer + nodebegin + kGmsh41HeaderSize + componentSize*numNodes;
    for (size_t i=0; i < 3*numNodes; i++) {
      double x;
      ReadFromBuffer(&x, pointbuffer + i*componentSize);
      point.push_back(x);
    }
  }

  size_t numElements, minElementTag, maxElementTag, numElementsInBlock;
  int elementType;

  // ascii elements
  if (binary == 0) {
    // convert element char buffer to stringstream
    buffer[elemend] = 0;
    stringstream ss(std::string(buffer + elembegin, elemend - elembegin));

    // read header
    ss >> numEntityBlocks >> numElements >> minElementTag >> maxElementTag;
    ss >> entityDim >> entityTag >> elementType >> numElementsInBlock;
    if (!ss.good()) {
      throw mjCError(NULL, "Error reading Elements header");
    }

    // require single block
    if (numEntityBlocks != 1 || numElements != numElementsInBlock) {
      throw mjCError(NULL, "All elements must be in single block");
    }

    // check number of elements is a positive number
    if (numElements < 0) {
      throw mjCError(NULL, "Invalid number of elements");
    }

    // dimensionality must be same as nodes
    if (entityDim != def.spec.flex->dim) {
      throw mjCError(NULL, "Inconsistent dimensionality in Elements");
    }

    if (numElements < 0 || numElements >= INT_MAX / 4) {
      throw mjCError(NULL, "Invalid numElements.");
    }

    // type must be consistent with dimensionality
    if ((entityDim == 1 && elementType != 1) ||
        (entityDim == 2 && elementType != 2) ||
        (entityDim == 3 && elementType != 4)) {
      throw mjCError(NULL, "Element type inconsistent with dimensionality");
    }

    // read elements, discard tags
    element.reserve((entityDim+1)*numElements);
    for (size_t i=0; i < numElements; i++) {
      size_t tag, nodeid;
      ss >> tag;
      for (int k=0; k <= entityDim; k++) {
        ss >> nodeid;
        if (!ss.good()) {
          throw mjCError(NULL, "Error reading Elements");
        }
        element.push_back((int)(nodeid-minNodeTag));
      }
    }
  }

  // binary elements
  else {
    // check header size
    if (elemend-elembegin < kGmsh41HeaderSize) {
      throw mjCError(NULL, "Invalid elements header");
    }

    // read header
    ReadFromBuffer(&numEntityBlocks, buffer + elembegin);
    ReadFromBuffer(&numElements, buffer + elembegin + 8);
    ReadFromBuffer(&minElementTag, buffer + elembegin + 16);
    ReadFromBuffer(&maxElementTag, buffer + elembegin + 24);
    ReadFromBuffer(&entityDim, buffer + elembegin + 32);
    ReadFromBuffer(&entityTag, buffer + elembegin + 36);
    ReadFromBuffer(&elementType, buffer + elembegin + 40);
    ReadFromBuffer(&numElementsInBlock, buffer + elembegin + 44);

    // require single block
    if (numEntityBlocks != 1 || numElements != numElementsInBlock) {
      throw mjCError(NULL, "All elements must be in single block");
    }

    // check number of elements is a positive number
    if (numElements < 0) {
      throw mjCError(NULL, "Invalid number of elements");
    }

    // dimensionality must be same as nodes
    if (entityDim != def.spec.flex->dim) {
      throw mjCError(NULL, "Inconsistent dimensionality in Elements");
    }

    // type must be consistent with dimensionality
    if ((entityDim == 1 && elementType != 1) ||
        (entityDim == 2 && elementType != 2) ||
        (entityDim == 3 && elementType != 4)) {
      throw mjCError(NULL, "Element type inconsistent with dimensionality");
    }

    if (numElements < 0 || numElements >= INT_MAX / 4) {
      throw mjCError(NULL, "Invalid numElements.");
    }

    // elementData: element tag and n node tags
    int numElementComponents = (entityDim+2);
    constexpr int componentSize = 8;
    int elementDataSize = numElementComponents*componentSize;

    // check section byte size
    if (elemend - elembegin < kGmsh41HeaderSize + numElements*elementDataSize) {
      throw mjCError(NULL, "Insufficient byte size of Elements");
    }

    // read elements, discard tags
    element.reserve((entityDim+1)*numElements);
    const char* elembuffer = buffer + elembegin + kGmsh41HeaderSize;
    for (size_t i=0; i < numElements; i++) {
      // skip element tag
      elembuffer += componentSize;

      // read vertex ids
      size_t elemid;
      for (int k=0; k <= entityDim; k++) {
        ReadFromBuffer(&elemid, elembuffer);
        int elementid = elemid - minNodeTag;
        element.push_back(elementid);
        elembuffer += componentSize;
      }
    }
  }
}



// load GMSH format 2.2
void mjCFlexcomp::LoadGMSH22(char* buffer, int binary, int nodeend,
                             int nodebegin, int elemend, int elembegin) {
  // number of nodes
  size_t numNodes = 0;

  // ascii nodes
  if (binary == 0) {
    // convert node char buffer to stringstream
    stringstream ss(std::string(buffer + nodebegin, nodeend - nodebegin));
    std::string line;

    // checking header template
    std::getline(ss, line);
    if (!IsValidElementOrNodeHeader22(line)) {
      throw mjCError(NULL, "Invalid node header");
    }
    ss.seekg(-(line.size()+1), std::ios::cur);

    // read header
    size_t maxNodeTag = 0;
    ss >> maxNodeTag;
    if (!ss.good()) {
      throw mjCError(NULL, "Error reading Nodes header");
    }
    numNodes = maxNodeTag;

    if (numNodes < 0 || numNodes >= INT_MAX / 3) {
      throw mjCError(NULL, "Invalid number of nodes.");
    }

    // read points, discard tag
    point.reserve(3*numNodes);
    for (size_t i=0; i < numNodes; i++) {
      size_t tag;
      double x;
      ss >> tag;
      if (!ss.good()) {
        throw mjCError(NULL, "Error reading node tags");
      }
      // reading nodes
      for (int k=0; k < 3; k++) {
        ss >> x;
        if (!ss.good()) {
          throw mjCError(NULL, "Error reading node coordinates");
        }
        point.push_back(x);
      }
    }
  }

  // binary nodes
  else {
    // header size for gmshApp
    constexpr int nodeHeaderSizeGmshApp = 5;
    // header size compatible with both gmshApp and Ftetwild
    constexpr int nodeHeaderSize = nodeHeaderSizeGmshApp - 1;
    // check header size
    if (nodeend-nodebegin < nodeHeaderSize) {
      throw mjCError(NULL, "Invalid nodes header");
    }

    // parse maxNodeTag and then cast it to int
    char maxNodeTagChar[11] = {0};
    ReadStrFromBuffer(maxNodeTagChar, buffer + nodebegin, std::min(10, nodeend - nodebegin));
    size_t measuredHeaderSize = strnlen(maxNodeTagChar, 10) - 1;
    size_t maxNodeTag;
    try {
      maxNodeTag = std::stoi(maxNodeTagChar);
    } catch (const std::out_of_range& e) {
      throw mjCError(NULL, "Invalid number of nodes");
    }
    numNodes = maxNodeTag;

    // check number of nodes is a positive number
    if (numNodes < 0) {
      throw mjCError(NULL, "Invalid number of nodes");
    }

    // node data: node tag and 3 nodes
    int nodeSize = sizeof(double);
    int indexSize = sizeof(int);
    int nodeDataSize = indexSize + 3*nodeSize;

    // check section byte size
    if (nodeend - nodebegin < nodeHeaderSize + numNodes*nodeDataSize) {
      throw mjCError(NULL, "Insufficient byte size of Nodes");
    }

    // read point, discard tag
    if (numNodes < 0 || numNodes >= INT_MAX / 3) {
      throw mjCError(NULL, "Invalid number of nodes.");
    }
    point.reserve(3*numNodes);
    // beginning of buffer containing node info
    const char* tagBuffer = buffer + nodebegin + measuredHeaderSize;
    for (int i=0; i < numNodes; i++) {
      int tag;
      int offset = i*(sizeof(int) + sizeof(double)*3);
      ReadFromBuffer(&tag, tagBuffer + offset);
      for (int k=0; k < 3; k++) {
        double x;
        const char* nodeBuffer = tagBuffer + sizeof(int) + sizeof(double)*k;
        ReadFromBuffer(&x, nodeBuffer + offset);
        point.push_back(x);
      }
    }
  }


  // ascii elements
  if (binary == 0) {
    // convert element char buffer to stringstream
    buffer[elemend] = 0;
    stringstream ss(std::string(buffer + elembegin, elemend - elembegin));
    std::string line;

    // checking header template
    std::getline(ss, line);
    if (!IsValidElementOrNodeHeader22(line)) {
      throw mjCError(NULL, "Invalid elements header");
    }
    ss.seekg(-(line.size()+1), std::ios::cur);
    // read header
    size_t maxElementTag = 0;
    ss >> maxElementTag;
    if (!ss.good()) {
      throw mjCError(NULL, "Error reading Elements header");
    }
    size_t numElements = maxElementTag;

    if (numElements < 0 || numElements >= INT_MAX / 4) {
      throw mjCError(NULL, "Invalid number of elements.");
    }


    // check number of elements is a positive number
    if (numElements < 0) {
      throw mjCError(NULL, "Invalid number of elements");
    }

    // reading first element's type
    int tag = 0, elementType = 0, numTags = 0;
    ss >> tag >> elementType >> numTags;
    if (!ss.good()) {
      throw mjCError(NULL, "Error reading Elements");
    }

    size_t entityDim = 0;
    int numNodeTags = 0;
    // surface
    if (elementType == 2) {
      entityDim = 2;
      numNodeTags = 3;
    }
    // tetrahedral
    else if (elementType == 4) {
      entityDim = 3;
      numNodeTags = 4;
    }

    if (numNodeTags < 1 || numNodeTags > 4) {
      throw mjCError(NULL, "Invalid number of node tags");
    }

    // setting entityDim
    def.spec.flex->dim = entityDim;

    // read elements, discard all tags
    element.reserve(numNodeTags*numElements);
    for (size_t i=0; i < numElements; i++) {
      int nodeTag = 0, physicalEntityTag = 0, elementModelEntityTag = 0;
      if (i != 0) {
        ss >> tag >> elementType >> numTags;
        if (!ss.good()) {
          throw mjCError(NULL, "Error reading Elements");
        }
      }
      if (numTags > 0) {
        ss >> physicalEntityTag >> elementModelEntityTag;
        if (!ss.good()) {
          throw mjCError(NULL, "Error reading Elements");
        }
      }
      for (int k=0; k < numNodeTags; k++) {
        ss >> nodeTag;
        if (!ss.good()) {
          throw mjCError(NULL, "Error reading Elements");
        }
        if (nodeTag > numNodes || nodeTag < 1) {
          throw mjCError(NULL, "Invalid node tag");
        }
        element.push_back((int)(nodeTag-1));
      }
    }
  }
  // binary elements
  else {
    // header size for gmshApp
    constexpr int elementHeaderSizeGmshApp = 4;
    // header size for Ftetwild
    constexpr int elementHeaderSizeFtetwild = 17;
    // check header size
    if (elemend - elembegin < elementHeaderSizeGmshApp) {
      throw mjCError(NULL, "Invalid elements header");
    }

    // reading elements
    char maxElementTagChar[11] = {0};
    ReadStrFromBuffer(maxElementTagChar, buffer + elembegin, std::min(10, elemend - elembegin));
    int measuredHeaderSize = strnlen(maxElementTagChar, 10) - 1;
    int maxElementTag;
    try {
      maxElementTag = std::stoi(maxElementTagChar);
    } catch (const std::out_of_range& e) {
      throw mjCError(NULL, "Invalid number of elements");
    }
    int numElements = maxElementTag;
    int tag, numTags;
    int nodeTag;
    int elementType;

    // check number of elements is a positive number
    if (numElements < 0) {
      throw mjCError(NULL, "Invalid number of elements");
    }

    // size of single component in element data
    int componentSize = sizeof(int);
    // element buffer
    const char* elementsBuffer = buffer + elembegin + measuredHeaderSize;
    ReadFromBuffer(&elementType, elementsBuffer);
    ReadFromBuffer(&numTags, elementsBuffer + componentSize*2);
    ReadFromBuffer(&tag, elementsBuffer + componentSize*3);

    // tetrahedral has 4 node tags and surface has 3
    int numNodeTags = 0;
    size_t entityDim = 0;
    // surface
    if (elementType == 2) {
      entityDim = 2;
      numNodeTags = 3;
    }
    // tetrahedral
    else if (elementType == 4) {
      entityDim = 3;
      numNodeTags = 4;
    }

    if (numNodeTags < 1 || numNodeTags > 4) {
      throw mjCError(NULL, "Invalid number of node tags");
    }

    def.spec.flex->dim = entityDim;

    // element data(Ftetwild): tag and 4 nodeTag
    constexpr int numComponentsFtetwild = 5;
    // element data(gmshApp): 4 Info components, 2 entity tag and entityDim+1 nodeTags
    constexpr int numInfoComponents = 4;
    constexpr int numEntityTagComponents = 2;

    int numComponentsGmshApp = numInfoComponents + numEntityTagComponents + numNodeTags;

    // single element data size
    int elementDataSizeFtetwild = numComponentsFtetwild*componentSize;
    int elementDataSizeGmshApp = numComponentsGmshApp*componentSize;

    // elements section buffer size
    int elementsBufferSizeFtetwild = elementHeaderSizeFtetwild +
                                     numElements*elementDataSizeFtetwild;
    int elementsBufferSizeGmshApp = elementHeaderSizeGmshApp +
                                    numElements*elementDataSizeGmshApp;

    // check section byte size for ftetwild
    if (elemend - elembegin < elementsBufferSizeFtetwild) {
      throw mjCError(NULL, "Insufficient byte size of Elements");
    }

    // Handling elements produced by gmsh
    if (numTags > 0) {
      // check section byte size for gmsh
      if (elemend - elembegin < elementsBufferSizeGmshApp) {
        throw mjCError(NULL, "Insufficient byte size of Elements");
      }

      // read first element
      for (int k =0; k < numNodeTags; k++) {
        ReadFromBuffer(&nodeTag, elementsBuffer + componentSize*(6+k));
        if (nodeTag > numNodes || nodeTag < 1) {
          throw mjCError(NULL, "Invalid node tag");
        }
        element.push_back(nodeTag-1);
      }

      // read every other element
      for (int i=1; i < numElements; i++) {
        const char* numTagsBuffer = elementsBuffer + componentSize*2;
        const char* tagBuffer = elementsBuffer + componentSize*3;
        int offset = i*elementDataSizeGmshApp;
        ReadFromBuffer(&numTags, numTagsBuffer + offset);
        ReadFromBuffer(&tag, tagBuffer+offset);
        for (int k =0; k < numNodeTags; k++) {
          const char* nodeTagBuffer = elementsBuffer + componentSize*(6+k);
          ReadFromBuffer(&nodeTag, nodeTagBuffer + offset);
          if (nodeTag > numElements || nodeTag < 1) {
            throw mjCError(NULL, "Invalid node tag");
          }
          element.push_back(nodeTag-1);
        }
      }
    }

    // Handling elements produced by ftetwild
    else {
      // read first element
      for (int k = 0; k < numNodeTags; k++) {
        const char* nodeTagBuffer = elementsBuffer + componentSize*(4+k);
        ReadFromBuffer(&nodeTag, nodeTagBuffer);
        if (nodeTag > numNodes || nodeTag < 1) {
          throw mjCError(NULL, "Invalid node tag");
        }
        element.push_back(nodeTag-1);
      }

      // read every other element
      for (int i=0; i < numElements-1; i++) {
        int offset = componentSize*(4+2) + i*elementDataSizeFtetwild;
        const char* tagBuffer = elementsBuffer + componentSize*2;
        ReadFromBuffer(&tag, tagBuffer + offset);
        for (int k=0; k < numNodeTags; k++) {
          const char* nodeTagBuffer = elementsBuffer + componentSize*(3+k);
          ReadFromBuffer(&nodeTag, nodeTagBuffer + offset);
          if (nodeTag > numElements || nodeTag < 1) {
            throw mjCError(NULL, "Invalid node tag");
          }
          element.push_back(nodeTag-1);
        }
      }
    }
  }
}



// load GMSH file from resource
void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) {
  // get buffer from resource
  char* buffer = 0;
  int buffer_sz = mju_readResource(resource, (const void**) &buffer);

  // check buffer
  if (buffer_sz < 0) {
    throw mjCError(NULL, "Could not read GMSH file");
  } else if (buffer_sz == 0) {
    throw mjCError(NULL, "Empty GMSH file");
  } else if (buffer_sz < 11 || strncmp(buffer, "$MeshFormat", 11)) {
    throw mjCError(NULL, "GMSH file must begin with $MeshFormat");
  }

  // check version, determine ascii or binary
  double version;
  int binary;
  if (sscanf(buffer + 11, "%lf %d", &version, &binary) != 2) {
    throw mjCError(NULL, "Could not read GMSH file header");
  }
  if (mju_round(100*version) != 220 && mju_round(100*version) != 410) {
    throw mjCError(NULL, "Only GMSH file format versions 4.1 and 2.2 are supported");
  }

  // find section begin/end
  int nodebegin = findstring(buffer, buffer_sz, "$Nodes");
  int nodeend =   findstring(buffer, buffer_sz, "$EndNodes");
  int elembegin = findstring(buffer, buffer_sz, "$Elements");
  int elemend =   findstring(buffer, buffer_sz, "$EndElements");


  // correct begin for string size, +1 for LF in binary (CRLF in Win ascii works)
  nodebegin += (int)strlen("$Nodes") + 1;
  elembegin += (int)strlen("$Elements") + 1;

  // check sections
  if (nodebegin < 0) {
    throw mjCError(NULL, "GMSH file missing $Nodes");
  }
  if (nodeend < nodebegin) {
    throw mjCError(NULL, "GMSH file missing $EndNodes after $Nodes");
  }
  if (elembegin < 0) {
    throw mjCError(NULL, "GMSH file missing $Elements");
  }
  if (elemend < elembegin) {
    throw mjCError(NULL, "GMSH file missing $EndElements after $Elements");
  }

  // Support for 4.1
  if (mju_round(100*version) == 410) {
    LoadGMSH41(buffer, binary, nodeend, nodebegin, elemend, elembegin);
  }

  // Support for 2.2
  else if (mju_round(100*version) == 220) {
    LoadGMSH22(buffer, binary, nodeend, nodebegin, elemend, elembegin);
  } else {
    throw mjCError(NULL, "Unsupported GMSH file format version");
  }
}