mujoco

// 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.

// Tests for user/user_api.cc.

#include <algorithm>
#include <array>
#include <cctype>
#include <cstddef>
#include <cstdint>
#include <filesystem>  // NOLINT
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/strings/match.h>
#include "src/cc/array_safety.h"
#include <mujoco/mujoco.h>
#include <mujoco/mjspec.h>
#include "src/xml/xml_api.h"
#include "src/xml/xml_numeric_format.h"
#include "test/fixture.h"

namespace mujoco {
namespace {

using ::testing::HasSubstr;
using ::testing::NotNull;
using ::testing::IsNull;


// -------------------------- test model manipulation  -------------------------

TEST_F(MujocoTest, GetSetData) {
  mjSpec* spec = mj_makeSpec();
  mjsBody* world = mjs_findBody(spec, "world");
  mjsBody* body = mjs_addBody(world, 0);
  mjsSite* site = mjs_addSite(body, 0);

  {
    double vec[10] = {0, 1, 2, 3, 4, 5, 6, 7, 8, 9};
    const char* str = "sitename";

    mjs_setName(site->element, str);
    mjs_setDouble(site->userdata, vec, 10);
  }

  EXPECT_THAT(mjs_getName(site->element)->c_str(), HasSubstr("sitename"));

  int nsize;
  const double* vec = mjs_getDouble(site->userdata, &nsize);
  for (int i = 0; i < nsize; ++i) {
    EXPECT_EQ(vec[i], i);
  }

  mj_deleteSpec(spec);
}
TEST_F(MujocoTest, TreeTraversal) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body1">
        <site name="site1"/>
        <body name="body2">
          <site name="site4"/>
        </body>
        <geom name="geom1" size="1"/>
        <geom name="geom2" size="1"/>
        <site name="site2"/>
        <site name="site3"/>
        <geom name="geom3" size="1"/>
      </body>
      <body name="body3">
        <site name="site5"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  std::array<char, 1000> err;
  mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
  ASSERT_THAT(spec, NotNull()) << err.data();

  mjsBody* world = mjs_findBody(spec, "world");
  mjsBody* body1 = mjs_findBody(spec, "body1");
  mjsBody* body2 = mjs_findBody(spec, "body2");
  mjsBody* body3 = mjs_findBody(spec, "body3");
  mjsElement* b1 = body1->element;
  mjsElement* b2 = body2->element;
  mjsElement* b3 = body3->element;
  mjsElement* site1 = mjs_findElement(spec, mjOBJ_SITE, "site1");
  mjsElement* site2 = mjs_findElement(spec, mjOBJ_SITE, "site2");
  mjsElement* site3 = mjs_findElement(spec, mjOBJ_SITE, "site3");
  mjsElement* site4 = mjs_findElement(spec, mjOBJ_SITE, "site4");
  mjsElement* site5 = mjs_findElement(spec, mjOBJ_SITE, "site5");
  mjsElement* geom1 = mjs_findElement(spec, mjOBJ_GEOM, "geom1");
  mjsElement* geom2 = mjs_findElement(spec, mjOBJ_GEOM, "geom2");
  mjsElement* geom3 = mjs_findElement(spec, mjOBJ_GEOM, "geom3");

  // test nonexistent
  EXPECT_EQ(mjs_firstElement(spec, mjOBJ_ACTUATOR), nullptr);
  EXPECT_EQ(mjs_firstElement(spec, mjOBJ_LIGHT), nullptr);
  EXPECT_EQ(mjs_firstChild(body1, mjOBJ_CAMERA, /*recurse=*/true), nullptr);
  EXPECT_EQ(mjs_firstChild(body1, mjOBJ_TENDON, /*recurse=*/true), nullptr);

  // test first, nonrecursive
  EXPECT_EQ(mjs_firstElement(spec, mjOBJ_BODY), world->element);
  EXPECT_EQ(site1, mjs_firstElement(spec, mjOBJ_SITE));
  EXPECT_EQ(site1, mjs_firstChild(body1, mjOBJ_SITE, /*recurse=*/false));
  EXPECT_EQ(geom1, mjs_firstChild(body1, mjOBJ_GEOM, /*recurse=*/false));
  EXPECT_EQ(site4, mjs_firstChild(body2, mjOBJ_SITE, /*recurse=*/false));
  EXPECT_EQ(site5, mjs_firstChild(body3, mjOBJ_SITE, /*recurse=*/false));

  // test first, recursive
  EXPECT_EQ(site1, mjs_firstChild(world, mjOBJ_SITE, /*recurse=*/true));
  EXPECT_EQ(geom1, mjs_firstChild(world, mjOBJ_GEOM, /*recurse=*/true));
  EXPECT_EQ(site4, mjs_firstChild(body2, mjOBJ_SITE, /*recurse=*/true));
  EXPECT_EQ(site5, mjs_firstChild(body3, mjOBJ_SITE, /*recurse=*/true));

  // test next, nonrecursive
  EXPECT_EQ(site2, mjs_nextChild(body1, site1, /*recursive=*/false));
  EXPECT_EQ(site3, mjs_nextChild(body1, site2, /*recursive=*/false));
  EXPECT_EQ(nullptr, mjs_nextChild(body1, site3, /*recursive=*/false));
  EXPECT_EQ(geom2, mjs_nextChild(body1, geom1, /*recursive=*/false));
  EXPECT_EQ(geom3, mjs_nextChild(body1, geom2, /*recursive=*/false));
  EXPECT_EQ(nullptr, mjs_nextChild(body1, geom3, /*recursive=*/false));
  EXPECT_EQ(mjs_nextElement(spec, site1), site2);
  EXPECT_EQ(mjs_nextElement(spec, site2), site3);
  EXPECT_EQ(mjs_nextElement(spec, site3), site4);
  EXPECT_EQ(mjs_nextElement(spec, site4), site5);
  EXPECT_EQ(mjs_nextElement(spec, site5), nullptr);
  EXPECT_EQ(mjs_nextElement(spec, geom1), geom2);
  EXPECT_EQ(mjs_nextElement(spec, geom2), geom3);
  EXPECT_EQ(mjs_nextElement(spec, geom3), nullptr);

  // test next, recursive
  EXPECT_EQ(b2, mjs_nextChild(world, b1, /*recursive=*/true));
  EXPECT_EQ(b3, mjs_nextChild(world, b2, /*recursive=*/true));
  EXPECT_EQ(site2, mjs_nextChild(body1, site1, /*recursive=*/true));
  EXPECT_EQ(site3, mjs_nextChild(body1, site2, /*recursive=*/true));
  EXPECT_EQ(site4, mjs_nextChild(body1, site3, /*recursive=*/true));
  EXPECT_EQ(site4, mjs_nextChild(world, site3, /*recursive=*/true));
  EXPECT_EQ(site5, mjs_nextChild(world, site4, /*recursive=*/true));
  EXPECT_EQ(nullptr, mjs_nextChild(body1, site5, /*recursive=*/true));
  EXPECT_EQ(geom2, mjs_nextChild(body1, geom1, /*recursive=*/true));
  EXPECT_EQ(geom3, mjs_nextChild(body1, geom2, /*recursive=*/true));
  EXPECT_EQ(nullptr, mjs_nextChild(body1, geom3, /*recursive=*/true));

  // check compilation ordering of sites
  mjModel* model = mj_compile(spec, nullptr);
  EXPECT_THAT(model, NotNull());
  EXPECT_EQ(mjs_getId(site1), 0);
  EXPECT_EQ(mjs_getId(site2), 1);
  EXPECT_EQ(mjs_getId(site3), 2);
  EXPECT_EQ(mjs_getId(site4), 3);
  EXPECT_EQ(mjs_getId(site5), 4);
  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, ActivatePlugin) {
  mjSpec* spec = mj_makeSpec();
  mjs_activatePlugin(spec, "mujoco.elasticity.cable");

  // associate plugin to body
  mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
  mjs_setString(body->plugin.plugin_name, "mujoco.elasticity.cable");
  body->plugin.element = mjs_addPlugin(spec)->element;
  body->plugin.active = true;
  mjsGeom* geom = mjs_addGeom(body, 0);
  geom->type = mjGEOM_BOX;
  geom->size[0] = 1;
  geom->size[1] = 1;
  geom->size[2] = 1;

  // compile and check that the plugin is present
  mjModel* model = mj_compile(spec, NULL);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nplugin, 1);
  EXPECT_THAT(model->body_plugin[1], 0);

  mj_deleteSpec(spec);
  mj_deleteModel(model);
}

TEST_F(MujocoTest, DeletePlugin) {
  mjSpec* spec = mj_makeSpec();
  mjs_activatePlugin(spec, "mujoco.pid");

  // create body
  mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
  mjsJoint* joint = mjs_addJoint(body, 0);
  mjsGeom* geom = mjs_addGeom(body, 0);
  mjs_setName(joint->element, "j1");
  joint->type = mjJNT_SLIDE;
  geom->size[0] = 1;

  // add actuator
  mjsActuator* actuator = mjs_addActuator(spec, 0);
  mjs_setString(actuator->target, "j1");
  mjs_setString(actuator->plugin.plugin_name, "mujoco.pid");
  actuator->plugin.element = mjs_addPlugin(spec)->element;
  actuator->plugin.active = true;
  actuator->trntype = mjTRN_JOINT;

  // compile and check that the plugin is present
  mjModel* model = mj_compile(spec, NULL);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nu, 1);
  EXPECT_THAT(model->nplugin, 1);
  EXPECT_THAT(model->actuator_plugin[0], 0);

  // delete actuator
  mjs_delete(spec, actuator->element);

  // recompile and check that the plugin is not present
  mjModel* newmodel = mj_compile(spec, NULL);
  EXPECT_THAT(newmodel, NotNull());
  EXPECT_THAT(newmodel->nu, 0);
  EXPECT_THAT(newmodel->nplugin, 0);

  mj_deleteSpec(spec);
  mj_deleteModel(model);
  mj_deleteModel(newmodel);
}

static constexpr char xml_plugin_1[] = R"(
  <mujoco model="MuJoCo Model">
    <worldbody>
      <body name="body"/>
    </worldbody>
  </mujoco>)";

static constexpr char xml_plugin_2[] = R"(
  <mujoco model="MuJoCo Model">
    <extension>
      <plugin plugin="mujoco.pid">
        <instance name="actuator-1">
          <config key="ki" value="4.0"/>
          <config key="slewmax" value="3.14159"/>
        </instance>
      </plugin>
    </extension>
    <worldbody>
      <body name="empty"/>
      <body name="body">
        <joint name="joint"/>
        <geom size="0.1"/>
      </body>
    </worldbody>
    <actuator>
      <plugin name="actuator-1" plugin="mujoco.pid" instance="actuator-1"
              joint="joint" actdim="2"/>
    </actuator>
  </mujoco>)";

TEST_F(MujocoTest, AttachPlugin) {
  std::array<char, 1000> err;
  mjSpec* parent = mj_parseXMLString(xml_plugin_1, 0, err.data(), err.size());
  ASSERT_THAT(parent, NotNull()) << err.data();
  mjSpec* spec_1 = mj_parseXMLString(xml_plugin_2, 0, err.data(), err.size());
  ASSERT_THAT(spec_1, NotNull()) << err.data();

  // do a copy before attaching
  mjSpec* spec_2 = mj_copySpec(spec_1);
  mjs_setString(spec_2->modelname, "first_copy");
  mjSpec* spec_3 = mj_copySpec(spec_1);
  mjs_setString(spec_3->modelname, "second_copy");
  ASSERT_THAT(spec_3, NotNull()) << err.data();

  // attach a body referencing the plugin to the frame and compile
  mjsBody* body_1 = mjs_findBody(parent, "body");
  EXPECT_THAT(body_1, NotNull());
  mjsFrame* attachment_frame = mjs_addFrame(body_1, 0);
  EXPECT_THAT(attachment_frame, NotNull());
  mjs_attach(attachment_frame->element, mjs_findBody(spec_1, "body")->element,
             "child-", "");
  mjModel* model_1 = mj_compile(parent, nullptr);
  EXPECT_THAT(model_1, NotNull());
  EXPECT_THAT(model_1->nbody, 3);

  // attach it a second time to test namespacing and compile
  ASSERT_THAT(spec_2, NotNull()) << err.data();
  mjs_attach(attachment_frame->element, mjs_findBody(spec_2, "body")->element,
             "copy-", "");
  mjModel* model_2 = mj_compile(parent, nullptr);
  EXPECT_THAT(model_2, NotNull());
  EXPECT_THAT(model_2->nbody, 4);

  // attach a body not referencing the plugin and compile
  mjs_attach(attachment_frame->element, mjs_findBody(spec_3, "empty")->element,
             "empty-", "");
  mjModel* model_3 = mj_compile(parent, nullptr);
  EXPECT_THAT(model_3, NotNull());
  EXPECT_THAT(model_3->nbody, 5);

  mj_deleteModel(model_1);
  mj_deleteModel(model_2);
  mj_deleteModel(model_3);
  mj_deleteSpec(parent);
  mj_deleteSpec(spec_1);
  mj_deleteSpec(spec_2);
  mj_deleteSpec(spec_3);
}

TEST_F(MujocoTest, DetachPlugin) {
  std::array<char, 1000> err;
  mjSpec* parent = mj_parseXMLString(xml_plugin_1, 0, err.data(), err.size());
  ASSERT_THAT(parent, NotNull()) << err.data();
  mjSpec* child = mj_parseXMLString(xml_plugin_2, 0, err.data(), err.size());
  ASSERT_THAT(child, NotNull()) << err.data();

  // attach a body referencing the plugin to the frame
  mjsElement* frame = mjs_addFrame(mjs_findBody(parent, "world"), 0)->element;
  mjsElement* body = mjs_findBody(child, "body")->element;
  EXPECT_THAT(mjs_attach(frame, body, "child-", ""), NotNull());

  // detach the body and compile
  mjsBody* body_to_detach = mjs_findBody(parent, "child-body");
  EXPECT_THAT(body_to_detach, NotNull());
  EXPECT_THAT(mjs_delete(parent, body_to_detach->element), 0);
  mjModel* model = mj_compile(parent, nullptr);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nbody, 2);
  EXPECT_THAT(model->nplugin, 0);

  mj_deleteModel(model);
  mj_deleteSpec(parent);
  mj_deleteSpec(child);
}

TEST_F(MujocoTest, AttachExplicitPlugin) {
  static constexpr char xml_parent[] = R"(
    <mujoco model="MuJoCo Model">
      <worldbody>
        <body name="body"/>
      </worldbody>
    </mujoco>)";

  std::array<char, 1000> err;
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, err.data(), err.size());
  ASSERT_THAT(parent, NotNull()) << err.data();

  mjSpec* child = mj_makeSpec();
  mjsBody* body = mjs_addBody(mjs_findBody(child, "world"), 0);
  mjsGeom* geom = mjs_addGeom(body, 0);
  mjsSite* site = mjs_addSite(body, 0);
  mjsSensor* sensor = mjs_addSensor(child);
  mjsPlugin* plugin = mjs_addPlugin(child);
  mjs_activatePlugin(child, "mujoco.sensor.touch_grid");
  mjs_setString(plugin->plugin_name, "mujoco.sensor.touch_grid");
  mjs_setString(sensor->plugin.plugin_name, "mujoco.sensor.touch_grid");
  mjs_setName(body->element, "body");
  mjs_setName(sensor->element, "touch2");
  mjs_setString(sensor->objname, "touch2");
  mjs_setName(site->element, "touch2");
  geom->size[0] = 0.1;
  site->size[0] = 0.001;
  sensor->type = mjSENS_PLUGIN;
  sensor->objtype = mjOBJ_SITE;
  sensor->plugin.element = plugin->element;
  sensor->plugin.active = true;
  std::map<std::string, std::string, std::less<> > config_attribs;
  config_attribs["size"] = "8 12";
  config_attribs["fov"] = "10 13";
  config_attribs["gamma"] = "0";
  config_attribs["nchannel"] = "1";
  mjs_setPluginAttributes(plugin, &config_attribs);

  mjsBody* body_parent = mjs_findBody(parent, "body");
  EXPECT_THAT(body_parent, NotNull());
  mjsFrame* attachment_frame = mjs_addFrame(body_parent, 0);
  EXPECT_THAT(attachment_frame, NotNull());

  mjs_attach(attachment_frame->element, mjs_findBody(child, "body")->element,
             "child-", "");
  mjModel* model = mj_compile(parent, nullptr);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nplugin, 1);

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(model);
}

TEST_F(MujocoTest, ReplicatePlugin) {
  static constexpr char xml[] = R"(
    <mujoco>
      <extension>
        <plugin plugin="mujoco.sdf.torus">
          <instance name="torus" />
        </plugin>
      </extension>
      <asset>
        <mesh name="torus">
          <plugin instance="torus" />
        </mesh>
      </asset>
      <worldbody>
        <geom type="sdf" mesh="torus">
          <plugin instance="torus" />
        </geom>
        <replicate count="100">
          <body />
        </replicate>
      </worldbody>
    </mujoco>)";

  std::array<char, 1000> err;
  mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
  ASSERT_THAT(spec, NotNull()) << err.data();
  mjModel* model = mj_compile(spec, nullptr);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nplugin, 1);
  mj_deleteSpec(spec);
  mj_deleteModel(model);
}

TEST_F(MujocoTest, ReplicateExplicitPlugin) {
  static constexpr char xml[] = R"(
    <mujoco>
      <extension>
        <plugin plugin="mujoco.sensor.touch_grid"/>
      </extension>
      <worldbody>
        <body name="tactile_sensor_2">
          <replicate count="2" offset="0 0.1 0">
            <geom type="sphere" size=".0008" />
          </replicate>
          <site name="touch2" size="0.001"/>
        </body>
      </worldbody>
      <sensor>
        <plugin name="touch2" plugin="mujoco.sensor.touch_grid" objtype="site" objname="touch2">
          <config key="size" value="8 12"/>
          <config key="fov" value="10 13"/>
          <config key="gamma" value="0"/>
          <config key="nchannel" value="1"/>
        </plugin>
      </sensor>
    </mujoco>)";

  std::array<char, 1000> err;
  mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
  ASSERT_THAT(spec, NotNull()) << err.data();
  mjModel* model = mj_compile(spec, nullptr);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nplugin, 1);
  mj_deleteSpec(spec);
  mj_deleteModel(model);
}

TEST_F(MujocoTest, RecompileFails) {
  mjSpec* spec = mj_makeSpec();
  mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0);
  mjsGeom* geom = mjs_addGeom(body, 0);
  geom->type = mjGEOM_SPHERE;
  geom->size[0] = 1;

  mjModel* model = mj_compile(spec, 0);
  mjData* data = mj_makeData(model);

  mjsMaterial* mat1 = mjs_addMaterial(spec, 0);
  mjsMaterial* mat2 = mjs_addMaterial(spec, 0);
  mjs_setName(mat1->element, "yellow");
  mjs_setName(mat2->element, "yellow");

  EXPECT_EQ(mj_recompile(spec, 0, model, data), -1);
  EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'yellow' in material");

  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, ModifyShellInertiaFails) {
  static constexpr char xml[] = R"(
  <mujoco>
    <asset>
      <mesh name="example_mesh"
        vertex="0 0 0  1 0 0  0 1 0  1 1 1e-6"
        face="0 1 2  2 1 3" inertia="shell"/>
    </asset>
    <worldbody>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1000> err;
  mjSpec* spec = mj_parseXMLString(xml, 0, err.data(), err.size());
  ASSERT_THAT(spec, NotNull()) << err.data();

  // add a geom to spec
  mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "world"), nullptr);
  geom->type = mjGEOM_MESH;
  mjs_setString(geom->meshname, "example_mesh");
  geom->typeinertia = mjINERTIA_SHELL;

  mjModel* model = mj_compile(spec, nullptr);
  EXPECT_THAT(model, IsNull());
  EXPECT_THAT(mjs_getError(spec),
              HasSubstr("inertia should be specified in the mesh asset"));
  mj_deleteSpec(spec);
  mj_deleteModel(model);
}

// ------------------- test recompilation multiple files -----------------------

std::vector<std::string> GetRecompileTestModels() {
  std::vector<std::string> models;
  std::string ext(".xml");
  for (const auto& path : {GetTestDataFilePath("."), GetModelPath(".")}) {
    for (const auto& p : std::filesystem::recursive_directory_iterator(path)) {
      if (p.path().extension() == ext) {
        std::string xml = p.path().string();
        if (absl::StrContains(xml, "malformed_") ||
            absl::StrContains(xml, "_fail") ||
            absl::StrContains(xml, "touch_grid") ||
            absl::StrContains(xml, "perf") ||
            absl::StrContains(xml, "cow")) {
          continue;
        }
        models.push_back(xml);
      }
    }
  }
  return models;
}

class RecompileCompareTest : public MujocoTest,
                             public ::testing::WithParamInterface<std::string> {
 public:
};
TEST_P(RecompileCompareTest, RecompileCompare) {
  std::string xml = GetParam();
  std::string field = "";

  FullFloatPrecision increase_precision;

  // load spec
  std::array<char, 1000> err;
  mjSpec* s = mj_parseXML(xml.c_str(), 0, err.data(), err.size());

  if (!s) {
    GTEST_SKIP() << "Failed to load " << xml << ": " << err.data();
  }

  // copy spec
  mjSpec* s_copy = mj_copySpec(s);

  // compare signature
  EXPECT_EQ(s->element->signature, s_copy->element->signature) << xml;

  // compile twice and compare
  mjModel* m_old = mj_compile(s, nullptr);

  if (!m_old) {
    mj_deleteSpec(s);
    GTEST_SKIP() << "Failed to compile " << xml << ": " << mjs_getError(s);
  }

  mjModel* m_new = mj_compile(s, nullptr);
  mjModel* m_copy = mj_compile(s_copy, nullptr);

  // compare signature
  EXPECT_EQ(m_old->signature, m_new->signature) << xml;
  EXPECT_EQ(m_old->signature, m_copy->signature) << xml;

  ASSERT_THAT(m_new, NotNull())
      << "Failed to recompile " << xml << ": " << mjs_getError(s);
  ASSERT_THAT(m_copy, NotNull())
      << "Failed to compile " << xml << ": " << mjs_getError(s_copy);

  mjtNum tol = 0;

  EXPECT_LE(CompareModel(m_old, m_new, field), tol)
      << "Compiled and recompiled models are different!\n"
      << "Affected file " << xml << '\n'
      << "Different field: " << field << '\n';

  EXPECT_LE(CompareModel(m_old, m_copy, field), tol)
      << "Original and copied models are different!\n"
      << "Affected file " << xml << '\n'
      << "Different field: " << field << '\n';

  // copy to a new spec, compile and compare
  mjSpec* s_copy2 = mj_copySpec(s);
  mjModel* m_copy2 = mj_compile(s_copy2, nullptr);

  ASSERT_THAT(m_copy2, NotNull())
      << "Failed to compile " << xml << ": " << mjs_getError(s_copy2);

  EXPECT_LE(CompareModel(m_old, m_copy2, field), tol)
      << "Original and re-copied models are different!\n"
      << "Affected file " << xml << '\n'
      << "Different field: " << field << '\n';

  mj_deleteModel(m_new);
  mj_deleteModel(m_copy);
  mj_deleteModel(m_copy2);
  mj_deleteSpec(s_copy);
  mj_deleteSpec(s_copy2);
  mj_deleteSpec(s);
  mj_deleteModel(m_old);
}

INSTANTIATE_TEST_SUITE_P(
    AllModels, RecompileCompareTest,
    ::testing::ValuesIn(GetRecompileTestModels()),
    [](const ::testing::TestParamInfo<std::string>& info) {
      std::string name = std::filesystem::path(info.param).filename().string();
      std::replace_if(
          name.begin(), name.end(),
          [](char c) { return !std::isalnum(c); }, '_');
      return name + "_" + std::to_string(info.index);
    });

TEST_F(MujocoTest, RecompileEdit) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body>
        <freejoint/>
        <geom size=".1"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  std::array<char, 1000> er;
  mjSpec *spec = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(spec, NotNull()) << er.data();
  mjModel *m1 = mj_compile(spec, nullptr);
  EXPECT_THAT(m1, NotNull());

  // add a geom
  mjsBody *world = mjs_findBody(spec, "world");
  mjsGeom *geom = mjs_addGeom(world, nullptr);
  geom->size[0] = 1;

  // compile again
  mjModel *m2 = mj_compile(spec, nullptr);
  EXPECT_THAT(m2, NotNull());

  mj_deleteModel(m1);
  mj_deleteModel(m2);
  mj_deleteSpec(spec);
}

// ------------------- test cache with modified assets -------------------------

TEST_F(MujocoTest, RecompileCompareObjCache) {
  static constexpr char xml[] = R"(
  <mujoco>
    <asset>
      <mesh file="cube.obj"/>
    </asset>
    <worldbody>
      <geom type="mesh" mesh="cube"/>
    </worldbody>
  </mujoco>)";

  static constexpr char cube1[] = R"(
  v -0.500000 -0.500000  0.500000
  v  0.500000 -0.500000  0.500000
  v -0.500000  0.500000  0.500000
  v  0.500000  0.500000  0.500000
  v -0.500000  0.500000 -0.500000
  v  0.500000  0.500000 -0.500000
  v -0.500000 -0.500000 -0.500000
  v  0.500000 -0.500000 -0.500000)";

  static constexpr char cube2[] = R"(
  v -1 -1  1
  v  1 -1  1
  v -1  1  1
  v  1  1  1
  v -1  1 -1
  v  1  1 -1
  v -1 -1 -1
  v  1 -1 -1)";

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "cube.obj", cube1, sizeof(cube1));

  std::array<char, 1024> error;

  // load model once
  mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
  EXPECT_EQ(m->mesh_vert[0], -0.5);
  mj_deleteModel(m);

  // update cube.obj, load again
  mj_deleteFileVFS(vfs.get(), "cube.obj");
  mj_addBufferVFS(vfs.get(), "cube.obj", cube2, sizeof(cube2));
  m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
  EXPECT_EQ(m->mesh_vert[0], -1);
  mj_deleteModel(m);

  mj_deleteVFS(vfs.get());
}

// tiny RGB 2 x 3 PNG file
static constexpr uint8_t tex1[] = {
  0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d,
  0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02,
  0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00,
  0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x78, 0xc1, 0xc0, 0xc0,
  0xc0, 0xf0, 0xbf, 0xb8, 0xb8, 0x98, 0x81, 0xe1, 0x3f, 0xc3, 0xff, 0xff,
  0xff, 0xc5, 0xc4, 0xc4, 0x00, 0x46, 0xd7, 0x07, 0x7f, 0xd2, 0x52, 0xa1,
  0x41, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60,
  0x82
};

// previous PNG file, but rotated by 180 degrees
static constexpr uint8_t tex2[] = {
  0x89, 0x50, 0x4e, 0x47, 0x0d, 0x0a, 0x1a, 0x0a, 0x00, 0x00, 0x00, 0x0d,
  0x49, 0x48, 0x44, 0x52, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 0x00, 0x02,
  0x08, 0x02, 0x00, 0x00, 0x00, 0x12, 0x16, 0xf1, 0x4d, 0x00, 0x00, 0x00,
  0x1c, 0x49, 0x44, 0x41, 0x54, 0x08, 0xd7, 0x63, 0x10, 0x13, 0x13, 0xfb,
  0xff, 0xff, 0x3f, 0xc3, 0x7f, 0x06, 0x96, 0xd8, 0xd8, 0x58, 0x46, 0x46,
  0x86, 0x17, 0x0c, 0x0c, 0x00, 0x49, 0x22, 0x06, 0x44, 0xe4, 0x91, 0xb8,
  0x83, 0x00, 0x00, 0x00, 0x00, 0x49, 0x45, 0x4e, 0x44, 0xae, 0x42, 0x60,
  0x82
};

TEST_F(MujocoTest, RecompileComparePngCache) {
  static constexpr char xml[] = R"(
  <mujoco>
    <asset>
      <texture content_type="image/png" file="tex.png" type="2d"/>
      <material name="material" texture="tex"/>
    </asset>

    <worldbody>
      <geom type="plane" material="material" size="4 4 4"/>
    </worldbody>
  </mujoco>
)";

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "tex.png", tex1, sizeof(tex1));

  std::array<char, 1024> error;

  // load model once
  mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
  EXPECT_EQ(m->ntexdata, 18);  // w x h x rgb = 3 x 2 x 3
  mjtByte byte = m->tex_data[0];
  mj_deleteModel(m);

  // update tex.png, load again
  mj_deleteFileVFS(vfs.get(), "tex.png");
  mj_addBufferVFS(vfs.get(), "tex.png", tex2, sizeof(tex2));
  m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());
  EXPECT_NE(m->tex_data[0], byte);
  EXPECT_EQ(m->tex_data[15], byte);  // first pixel is now last pixel
  mj_deleteModel(m);

  mj_deleteVFS(vfs.get());
}

TEST_F(MujocoTest, DisableCache) {
  static constexpr char xml[] = R"(
  <mujoco>
    <asset>
      <texture content_type="image/png" file="tex.png" type="2d"/>
      <material name="material" texture="tex"/>
    </asset>

    <worldbody>
      <geom type="plane" material="material" size="4 4 4"/>
    </worldbody>
  </mujoco>
)";

  mjCache* cache = mj_getCache();
  std::size_t capacity = mj_getCacheCapacity(cache);
  mj_setCacheCapacity(cache, 0);

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "tex.png", tex1, sizeof(tex1));

  std::array<char, 1024> error;

  // load model once
  mjModel* m = LoadModelFromString(xml, error.data(), error.size(), vfs.get());

  EXPECT_EQ(mj_getCacheSize(cache), 0);
  EXPECT_EQ(m->ntexdata, 18);  // w x h x rgb = 3 x 2 x 3

  mj_setCacheCapacity(cache, capacity);

  mj_deleteModel(m);
  mj_deleteVFS(vfs.get());
}

// -------------------------------- test textures ------------------------------

TEST_F(MujocoTest, TextureFromBuffer) {
  mjSpec* spec = mj_makeSpec();

  mjsTexture* t1 = mjs_addTexture(spec);
  mjs_setName(t1->element, "tex1");
  t1->type = mjTEXTURE_2D;
  t1->width = 3;
  t1->height = 2;
  t1->nchannel = 3;
  mjs_setBuffer(t1->data, (std::byte*)tex1, 18);

  mjsTexture* t2 = mjs_addTexture(spec);
  mjs_setName(t2->element, "tex2");
  t2->type = mjTEXTURE_2D;
  t2->width = 3;
  t2->height = 2;
  t2->nchannel = 3;
  mjs_setBuffer(t2->data, (std::byte*)tex2, 18);

  mjsMaterial* mat = mjs_addMaterial(spec, nullptr);
  mjs_setName(mat->element, "mat");
  mjs_setInStringVec(mat->textures, mjTEXROLE_RGB, "tex1");
  mjs_setInStringVec(mat->textures, mjTEXROLE_ORM, "tex2");

  mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "world"), nullptr);
  mjs_setString(geom->material, "mat");
  geom->size[0] = 1;

  mjModel* m = mj_compile(spec, nullptr);
  EXPECT_THAT(m, NotNull());
  EXPECT_EQ(m->ntex, 2);

  std::array<char, 1024> err;
  std::array<char, 1024> str;
  mj_saveXMLString(spec, str.data(), str.size(), err.data(), err.size());
  EXPECT_STREQ(err.data(), "XML Error: no support for buffer textures.");

  mj_deleteModel(m);
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, TestTextureFlip) {
  mjSpec* spec = mj_makeSpec();
  EXPECT_THAT(spec, NotNull());

  std::vector<std::byte> texture_data = {std::byte{1}, std::byte{2},
                                         std::byte{3}, std::byte{4}};
  mjsTexture* texture = mjs_addTexture(spec);
  mjs_setName(texture->element, "hflipped");
  texture->type = mjTEXTURE_2D;
  texture->width = 2;
  texture->height = 2;
  texture->nchannel = 1;
  texture->hflip = true;
  mjs_setBuffer(texture->data, texture_data.data(), texture_data.size());

  texture_data = {std::byte{11}, std::byte{12}, std::byte{13}, std::byte{21},
                  std::byte{22}, std::byte{23}, std::byte{31}, std::byte{32},
                  std::byte{33}, std::byte{41}, std::byte{42}, std::byte{43}};

  texture = mjs_addTexture(spec);
  mjs_setName(texture->element, "vflipped");
  texture->type = mjTEXTURE_2D;
  texture->width = 2;
  texture->height = 2;
  texture->nchannel = 3;
  texture->vflip = true;
  mjs_setBuffer(texture->data, texture_data.data(), texture_data.size());

  mjModel* model = mj_compile(spec, 0);
  EXPECT_THAT(model, NotNull()) << mjs_getError(spec);

  EXPECT_THAT(model->tex_data[0], 2);
  EXPECT_THAT(model->tex_data[1], 1);
  EXPECT_THAT(model->tex_data[2], 4);
  EXPECT_THAT(model->tex_data[3], 3);

  EXPECT_THAT(model->tex_data[4], 31);
  EXPECT_THAT(model->tex_data[5], 32);
  EXPECT_THAT(model->tex_data[6], 33);
  EXPECT_THAT(model->tex_data[7], 41);
  EXPECT_THAT(model->tex_data[8], 42);
  EXPECT_THAT(model->tex_data[9], 43);
  EXPECT_THAT(model->tex_data[10], 11);
  EXPECT_THAT(model->tex_data[11], 12);
  EXPECT_THAT(model->tex_data[12], 13);
  EXPECT_THAT(model->tex_data[13], 21);
  EXPECT_THAT(model->tex_data[14], 22);
  EXPECT_THAT(model->tex_data[15], 23);

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

// -------------------------------- test attach --------------------------------

static constexpr char xml_child[] = R"(
  <mujoco model="child">
    <default>
      <default class="cylinder">
        <geom type="cylinder" size=".1 1 0"/>
      </default>
    </default>

    <custom>
      <numeric data="10" name="constant"/>
    </custom>

    <asset>
      <texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
      <material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
    </asset>

    <worldbody>
      <frame name="pframe">
        <frame name="cframe">
          <body name="body">
            <joint type="hinge" name="hinge"/>
            <geom class="cylinder" material="material"/>
            <light mode="targetbody" target="targetbody"/>
            <site name="site" material="material"/>
            <body name="targetbody"/>
            <body/>
          </body>
        </frame>
      </frame>
      <body name="ignore">
        <geom size=".1"/>
        <joint type="slide"/>
      </body>
      <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
    </worldbody>

    <sensor>
      <framepos name="sensor" objtype="body" objname="body"/>
      <framepos name="ignore" objtype="body" objname="ignore"/>
    </sensor>

    <tendon>
      <fixed name="fixed">
        <joint joint="hinge" coef="2"/>
      </fixed>
    </tendon>

    <actuator>
      <position name="hinge" joint="hinge" timeconst=".01"/>
      <position name="fixed" tendon="fixed" timeconst=".01"/>
      <position name="site" site="site" timeconst=".01"/>
    </actuator>

    <contact>
      <exclude body1="body" body2="targetbody"/>
    </contact>

    <keyframe>
      <key name="two" time="2" qpos="2 22" act="1 2 3" ctrl="1 2 3"/>
      <key name="three" time="3" qpos="3 33" act="4 5 6" ctrl="4 5 6"/>
    </keyframe>
  </mujoco>)";

TEST_F(MujocoTest, AttachSame) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_result[] = R"(
  <mujoco model="child">
    <default>
      <default class="cylinder">
        <geom type="cylinder" size=".1 1 0"/>
      </default>
    </default>

    <custom>
      <numeric data="10" name="constant"/>
    </custom>

    <asset>
      <texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
      <material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
    </asset>

    <worldbody>
      <body name="body">
        <joint type="hinge" name="hinge"/>
        <geom class="cylinder" material="material"/>
        <light mode="targetbody" target="targetbody"/>
        <site name="site" material="material"/>
        <body name="targetbody"/>
        <body/>
      </body>
      <body name="ignore">
        <geom size=".1"/>
        <joint type="slide"/>
      </body>
      <frame name="frame" pos=".1 0 0" euler="0 90 0">
        <body name="attached-body-1">
          <joint type="hinge" name="attached-hinge-1"/>
          <geom class="cylinder" material="material"/>
          <light mode="targetbody" target="attached-targetbody-1"/>
          <site name="attached-site-1" material="material"/>
          <body name="attached-targetbody-1"/>
          <body/>
        </body>
      </frame>
    </worldbody>

    <sensor>
      <framepos name="sensor" objtype="body" objname="body"/>
      <framepos name="ignore" objtype="body" objname="ignore"/>
      <framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
    </sensor>

    <tendon>
      <fixed name="fixed">
        <joint joint="hinge" coef="2"/>
      </fixed>
      <fixed name="attached-fixed-1">
        <joint joint="attached-hinge-1" coef="2"/>
      </fixed>
    </tendon>

    <actuator>
      <position name="hinge" joint="hinge" timeconst=".01"/>
      <position name="fixed" tendon="fixed" timeconst=".01"/>
      <position name="site" site="site" timeconst=".01"/>
      <position name="attached-hinge-1" joint="attached-hinge-1" timeconst=".01"/>
      <position name="attached-fixed-1" tendon="attached-fixed-1" timeconst=".01"/>
      <position name="attached-site-1" site="attached-site-1" timeconst=".01"/>
    </actuator>

    <contact>
      <exclude body1="body" body2="targetbody"/>
      <exclude body1="attached-body-1" body2="attached-targetbody-1"/>
    </contact>

    <keyframe>
      <key name="two" time="2" qpos="2 22 0" act="1 2 3 0 0 0" ctrl="1 2 3 0 0 0"/>
      <key name="three" time="3" qpos="3 33 0" act="4 5 6 0 0 0" ctrl="4 5 6 0 0 0"/>
      <key name="attached-two-1" time="2" qpos="0 22 2" act="0 0 0 1 2 3" ctrl="0 0 0 1 2 3"/>
      <key name="attached-three-1" time="3" qpos="0 33 3" act="0 0 0 4 5 6" ctrl="0 0 0 4 5 6"/>
    </keyframe>
  </mujoco>)";

  // create parent
  mjSpec* parent = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjs_setDeepCopy(parent, true);  // needed for self-attach

  // get frame
  mjsFrame* frame = mjs_findFrame(parent, "frame");
  EXPECT_THAT(frame, NotNull());

  // get subtree
  mjsBody* body = mjs_findBody(parent, "body");
  EXPECT_THAT(body, NotNull());

  // attach child to parent frame
  mjsBody* attached =
      mjs_asBody(mjs_attach(frame->element, body->element, "attached-", "-1"));
  EXPECT_THAT(attached, mjs_findBody(parent, "attached-body-1"));

  // check that the spec was not copied
  EXPECT_THAT(mjs_findSpec(parent, "child"), IsNull());

  // compile new model
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull());

  // check full name stored in mjModel
  EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 5), "attached-body-1");

  // check body 3 is attached to the world
  EXPECT_THAT(m_attached->body_parentid[4], 0);

  // compare with expected XML
  mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';;

  // destroy everything
  mj_deleteSpec(parent);
  mj_deleteModel(m_attached);
  mj_deleteModel(m_expected);
}

TEST_F(MujocoTest, AttachSpatialTendonWithoutSidesite) {
  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent_body">
        <geom size="0.1" type="sphere"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="child_body">
        <geom name="wrap_geom" size="0.05" type="sphere"/>
        <site name="site_A" pos="0 0 0.1"/>
        <site name="site_B" pos="0 0 -0.1"/>
        <site name="side_site" pos="0.05 0 0"/>
      </body>
    </worldbody>
    <tendon>
      <spatial name="tendon_with_sidesite">
        <site site="site_A"/>
        <geom geom="wrap_geom" sidesite="side_site"/>
        <site site="site_B"/>
      </spatial>
      <spatial name="tendon_without_sidesite">
        <site site="site_A"/>
        <geom geom="wrap_geom"/>
        <site site="site_B"/>
      </spatial>
    </tendon>
  </mujoco>)";

  std::array<char, 1000> er;
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  ASSERT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  ASSERT_THAT(child, NotNull()) << er.data();

  mjsBody* parent_body = mjs_findBody(parent, "parent_body");
  ASSERT_THAT(parent_body, NotNull());
  mjsSite* attach_site = mjs_addSite(parent_body, 0);
  mjs_setName(attach_site->element, "attach_site");

  mjs_attach(attach_site->element,
             mjs_findBody(child, "child_body")->element, "", "_child");

  EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON,
                              "tendon_with_sidesite_child"), NotNull());
  EXPECT_THAT(mjs_findElement(parent, mjOBJ_TENDON,
                              "tendon_without_sidesite_child"), NotNull());

  mjModel* model = mj_compile(parent, nullptr);
  ASSERT_THAT(model, NotNull()) << mjs_getError(parent);
  EXPECT_EQ(model->ntendon, 2);

  mj_deleteModel(model);
  mj_deleteSpec(parent);
  mj_deleteSpec(child);
}

TEST_F(MujocoTest, AttachSpatialTendonGitHubIssue3119) {
  static constexpr char parent_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent_body">
        <geom size="0.1" type="sphere"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char child_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="child_body">
        <geom name="wrap_geom" size="0.05" type="sphere"/>
        <site name="site_A" pos="0 0 0.1"/>
        <site name="site_B" pos="0 0 -0.1"/>
        <site name="side_site" pos="0.05 0 0"/>
      </body>
    </worldbody>
    <tendon>
      <spatial name="tendon_with_sidesite">
        <site site="site_A"/>
        <geom geom="wrap_geom" sidesite="side_site"/>
        <site site="site_B"/>
      </spatial>
      <spatial name="tendon_without_sidesite">
        <site site="site_A"/>
        <geom geom="wrap_geom"/>
        <site site="site_B"/>
      </spatial>
    </tendon>
  </mujoco>)";

  std::array<char, 1000> er;
  mjSpec* parent_spec =
      mj_parseXMLString(parent_xml, 0, er.data(), er.size());
  ASSERT_THAT(parent_spec, NotNull()) << er.data();
  mjSpec* child_spec =
      mj_parseXMLString(child_xml, 0, er.data(), er.size());
  ASSERT_THAT(child_spec, NotNull()) << er.data();

  mjsBody* parent_body = mjs_findBody(parent_spec, "parent_body");
  ASSERT_THAT(parent_body, NotNull());
  mjsSite* attach_site = mjs_addSite(parent_body, 0);
  mjs_setName(attach_site->element, "attach_site");

  mjs_attach(attach_site->element,
             mjs_findBody(child_spec, "child_body")->element,
             "", "_child");

  EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
                              "tendon_with_sidesite_child"), NotNull());
  EXPECT_THAT(mjs_findElement(parent_spec, mjOBJ_TENDON,
                              "tendon_without_sidesite_child"), NotNull());

  mjModel* model = mj_compile(parent_spec, nullptr);
  ASSERT_THAT(model, NotNull()) << mjs_getError(parent_spec);
  EXPECT_EQ(model->ntendon, 2);

  mj_deleteModel(model);
  mj_deleteSpec(parent_spec);
  mj_deleteSpec(child_spec);
}

TEST_F(MujocoTest, AttachDifferent) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco model="parent">
    <default>
      <default class="geom_size">
        <geom size="0.1"/>
      </default>
    </default>

    <worldbody>
      <body name="sphere">
        <freejoint/>
        <geom class="geom_size"/>
        <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
      </body>
    </worldbody>

    <keyframe>
      <key name="one" time="1" qpos="1 1 1 1 0 0 0"/>
    </keyframe>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco model="parent">
    <default>
      <default class="geom_size">
        <geom size="0.1"/>
      </default>
      <default class="attached-cylinder-1">
        <geom type="cylinder" size=".1 1 0"/>
      </default>
    </default>

    <custom>
      <numeric data="10" name="attached-constant-1"/>
    </custom>

    <asset>
      <texture name="attached-texture-1" type="2d" builtin="checker" width="32" height="32"/>
      <material name="attached-material-1" texture="attached-texture-1" texrepeat="1 1" texuniform="true"/>
    </asset>

    <worldbody>
      <body name="sphere">
        <freejoint/>
        <geom class="geom_size"/>
        <frame name="frame" pos=".1 0 0" euler="0 90 0">
          <body name="attached-body-1">
            <joint type="hinge" name="attached-hinge-1"/>
            <geom class="attached-cylinder-1" material="attached-material-1"/>
            <light mode="targetbody" target="attached-targetbody-1"/>
            <site name="attached-site-1" material="attached-material-1"/>
            <body name="attached-targetbody-1"/>
            <body/>
          </body>
        </frame>
      </body>
    </worldbody>

    <sensor>
      <framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
    </sensor>

    <tendon>
      <fixed name="attached-fixed-1">
        <joint joint="attached-hinge-1" coef="2"/>
      </fixed>
    </tendon>

    <actuator>
      <position name="attached-hinge-1" joint="attached-hinge-1" timeconst=".01"/>
      <position name="attached-fixed-1" tendon="attached-fixed-1" timeconst=".01"/>
      <position name="attached-site-1" site="attached-site-1" timeconst=".01"/>
    </actuator>

    <contact>
      <exclude body1="attached-body-1" body2="attached-targetbody-1"/>
    </contact>

    <keyframe>
      <key name="one" time="1" qpos="1 1 1 1 0 0 0 0"/>
      <key name="attached-two-1" time="2" qpos="0 0 0 1 0 0 0 2" act="1 2 3" ctrl="1 2 3"/>
      <key name="attached-three-1" time="3" qpos="0 0 0 1 0 0 0 3" act="4 5 6" ctrl="4 5 6"/>
    </keyframe>
  </mujoco>)";

  // model with one free sphere and a frame
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();

  // get frame
  mjsFrame* frame = mjs_findFrame(parent, "frame");
  EXPECT_THAT(frame, NotNull());

  // model with one cylinder and a hinge
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  // get subtree
  mjsBody* body = mjs_findBody(child, "body");
  EXPECT_THAT(body, NotNull());

  // attach child to parent frame
  mjsBody* attached =
      mjs_asBody(mjs_attach(frame->element, body->element, "attached-", "-1"));
  EXPECT_THAT(attached, mjs_findBody(parent, "attached-body-1"));

  // check that the spec was copied
  EXPECT_THAT(mjs_findSpec(parent, "child"), NotNull());

  // compile new model
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull());

  // check frame is present
  EXPECT_THAT(mjs_findFrame(parent, "frame"), NotNull());

  // check full name stored in mjModel
  EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1");

  // check body 2 is attached to body 1
  EXPECT_THAT(m_attached->body_parentid[2], 1);

  // check that the correct defaults are present
  EXPECT_THAT(mjs_findDefault(parent, "main"), NotNull());
  EXPECT_THAT(mjs_findDefault(parent, "geom_size"), NotNull());
  EXPECT_THAT(mjs_findDefault(parent, "attached-cylinder-1"), NotNull());

  // compare with expected XML
  mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';;

  // destroy everything
  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(m_attached);
  mj_deleteModel(m_expected);
}

TEST_F(MujocoTest, AttachFrame) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco model="parent">
    <worldbody>
      <body name="sphere">
        <freejoint/>
        <geom size=".1"/>
        <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
      </body>
    </worldbody>

    <keyframe>
      <key name="one" time="1" qpos="1 1 1 1 0 0 0"/>
    </keyframe>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco model="parent">
    <default>
      <default class="attached-cylinder-1">
        <geom type="cylinder" size=".1 1 0"/>
      </default>
    </default>

    <custom>
      <numeric data="10" name="attached-constant-1"/>
    </custom>

    <asset>
      <texture name="attached-texture-1" type="2d" builtin="checker" width="32" height="32"/>
      <material name="attached-material-1" texture="attached-texture-1" texrepeat="1 1" texuniform="true"/>
    </asset>

    <worldbody>
      <body name="sphere">
        <freejoint/>
        <geom size=".1"/>
        <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
        <frame name="pframe">
          <frame name="cframe">
            <body name="attached-body-1">
              <joint type="hinge" name="attached-hinge-1"/>
              <geom class="attached-cylinder-1" material="attached-material-1"/>
              <light mode="targetbody" target="attached-targetbody-1"/>
              <site name="attached-site-1" material="attached-material-1"/>
              <body name="attached-targetbody-1"/>
              <body/>
            </body>
          </frame>
        </frame>
      </body>
    </worldbody>

    <sensor>
      <framepos name="attached-sensor-1" objtype="body" objname="attached-body-1"/>
    </sensor>

    <tendon>
      <fixed name="attached-fixed-1">
        <joint joint="attached-hinge-1" coef="2"/>
      </fixed>
    </tendon>

    <actuator>
      <position name="attached-hinge-1" joint="attached-hinge-1" timeconst=".01"/>
      <position name="attached-fixed-1" tendon="attached-fixed-1" timeconst=".01"/>
      <position name="attached-site-1" site="attached-site-1" timeconst=".01"/>
    </actuator>

    <contact>
      <exclude body1="attached-body-1" body2="attached-targetbody-1"/>
    </contact>

    <keyframe>
      <key name="one" time="1" qpos="1 1 1 1 0 0 0 0"/>
      <key name="attached-two-1" time="2" qpos="0 0 0 1 0 0 0 2" act="1 2 3" ctrl="1 2 3"/>
      <key name="attached-three-1" time="3" qpos="0 0 0 1 0 0 0 3" act="4 5 6" ctrl="4 5 6"/>
    </keyframe>
  </mujoco>)";

  // model with one free sphere and a frame
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();

  // get body
  mjsBody* body = mjs_findBody(parent, "sphere");
  EXPECT_THAT(body, NotNull());

  // model with one cylinder and a hinge
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  // get subtree
  mjsFrame* frame = mjs_findFrame(child, "pframe");
  EXPECT_THAT(frame, NotNull());

  // attach child frame to parent body
  mjsFrame* attached =
      mjs_asFrame(mjs_attach(body->element, frame->element, "attached-", "-1"));
  EXPECT_THAT(attached, mjs_findFrame(parent, "attached-pframe-1"));

  // check that the spec was copied
  EXPECT_THAT(mjs_findSpec(parent, "child"), NotNull());

  // check that the parent body was not namespaced
  EXPECT_THAT(mjs_findBody(child, "world"), NotNull());

  // compile new model
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull());

  // check full name stored in mjModel
  EXPECT_STREQ(mj_id2name(m_attached, mjOBJ_BODY, 2), "attached-body-1");

  // check body 2 is attached to body 1
  EXPECT_THAT(m_attached->body_parentid[2], 1);

  // compare with expected XML
  mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';;

  // destroy everything
  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(m_attached);
  mj_deleteModel(m_expected);
}

TEST_F(MujocoTest, AttachCompiled) {
  std::array<char, 1000> er;

  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <geom name="floor" pos="0 0 0" size="0 0 0.05" type="plane"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="base">
        <freejoint/>
        <geom name="geom1" size="0.1" type="sphere"/>
      </body>
    </worldbody>
  </mujoco>)";

  // load parent
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();

  // load child
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  // compile child
  mjModel* m_child = mj_compile(child, 0);
  EXPECT_THAT(m_child, NotNull()) << mjs_getError(child);

  // add frame to the parent to attach
  mjsBody* world = mjs_findBody(parent, "world");
  EXPECT_THAT(world, NotNull()) << mjs_getError(parent);
  mjsFrame* frame = mjs_addFrame(world, 0);
  EXPECT_THAT(frame, NotNull()) << mjs_getError(parent);

  // attach child body to the frame
  mjsBody* to_attach = mjs_findBody(child, "base");
  EXPECT_THAT(to_attach, NotNull()) << mjs_getError(child);
  mjs_attach(frame->element, to_attach->element, "", "");

  // check that attached model can be compiled
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull()) <<
      "Failed to compile attached model" <<  mjs_getError(parent);

  // destroy everything
  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(m_attached);
  mj_deleteModel(m_child);
}

void TestDetachBody(bool compile) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_result[] = R"(
  <mujoco model="child">
    <asset>
      <texture name="texture" type="2d" builtin="checker" width="32" height="32"/>
      <material name="material" texture="texture" texrepeat="1 1" texuniform="true"/>
    </asset>

    <custom>
      <numeric data="10" name="constant"/>
    </custom>

    <worldbody>
      <frame name="pframe">
        <frame name="cframe">
        </frame>
      </frame>
      <body name="ignore">
        <geom size=".1"/>
        <joint type="slide"/>
      </body>
      <frame name="frame" pos=".1 0 0" euler="0 90 0"/>
    </worldbody>

    <sensor>
      <framepos name="ignore" objtype="body" objname="ignore"/>
    </sensor>

    <keyframe>
      <key name="two" time="2" qpos="22"/>
      <key name="three" time="3" qpos="33"/>
    </keyframe>
  </mujoco>)";

  // model with one cylinder and a hinge
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  // compile model (for testing double compilation)
  mjModel* m_child = compile ? mj_compile(child, 0) : nullptr;

  // get subtree
  mjsBody* body = mjs_findBody(child, "body");
  EXPECT_THAT(body, NotNull());

  // delete subtree
  EXPECT_THAT(mjs_delete(child, body->element), 0);

  // try saving to XML before compiling again
  std::array<char, 1024> e;
  std::array<char, 1024> s;
  EXPECT_EQ(mj_saveXMLString(child, s.data(), 1024, e.data(), 1024), -1);
  EXPECT_THAT(e.data(), compile
                  ? HasSubstr("Model has pending keyframes")
                  : HasSubstr("Only compiled model can be written"));

  // compile new model
  mjModel* m_detached = mj_compile(child, 0);
  EXPECT_THAT(m_detached, NotNull());

  // compare with expected XML
  mjModel* m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_detached, m_expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  // destroy everything
  mj_deleteSpec(child);
  mj_deleteModel(m_detached);
  mj_deleteModel(m_expected);
  if (m_child) mj_deleteModel(m_child);
}

TEST_F(MujocoTest, DetachBody) {
  TestDetachBody(/*compile=*/false);
  TestDetachBody(/*compile=*/true);
}

TEST_F(MujocoTest, AttachToSite) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <site name="site" pos="1 0 0" quat="0 1 0 0"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="sphere">
        <joint type="slide"/>
        <geom size=".1"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco>
    <worldbody>
      <site name="site" pos="1 0 0" quat="0 1 0 0"/>
      <frame pos="1 0 0" quat="0 1 0 0">
        <body name="attached-sphere-1">
          <joint type="slide"/>
          <geom size=".1"/>
        </body>
      </frame>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  mjsBody* world = mjs_findBody(parent, "world");
  EXPECT_THAT(world, NotNull());
  mjsSite* site = mjs_asSite(mjs_firstChild(world, mjOBJ_SITE, 0));
  EXPECT_THAT(site, NotNull());
  mjsBody* body = mjs_findBody(child, "sphere");
  EXPECT_THAT(body, NotNull());
  mjsBody* attached =
      mjs_asBody(mjs_attach(site->element, body->element, "attached-", "-1"));
  EXPECT_THAT(attached, NotNull());

  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());
  mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(model);
  mj_deleteModel(expected);
}

TEST_F(MujocoTest, AttachFrameToSite) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <site name="site" pos="1 0 0" quat="0 1 0 0"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <frame name="frame">
        <body name="sphere">
          <joint type="slide"/>
          <geom size=".1"/>
        </body>
      </frame>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco>
    <worldbody>
      <site name="site" pos="1 0 0" quat="0 1 0 0"/>
      <frame pos="1 0 0" quat="0 1 0 0">
        <body name="attached-sphere-1">
          <joint type="slide"/>
          <geom size=".1"/>
        </body>
      </frame>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  mjsBody* world = mjs_findBody(parent, "world");
  EXPECT_THAT(world, NotNull());
  mjsSite* site = mjs_asSite(mjs_firstChild(world, mjOBJ_SITE, 0));
  EXPECT_THAT(site, NotNull());
  mjsFrame* frame = mjs_findFrame(child, "frame");
  EXPECT_THAT(frame, NotNull());
  mjsFrame* attached =
      mjs_asFrame(mjs_attach(site->element, frame->element, "attached-", "-1"));
  EXPECT_THAT(attached, NotNull());

  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());
  mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(model);
  mj_deleteModel(expected);
}

TEST_F(MujocoTest, BodyToFrame) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <frame name="frame" pos="1 2 3"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="sphere">
        <joint type="slide"/>
        <geom size=".1"/>
      </body>
      <camera pos="0 0 0" quat="1 0 0 0"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <frame name="frame" pos="1 2 3">
          <body name="attached-sphere-1">
            <joint type="slide"/>
            <geom size=".1"/>
          </body>
          <frame name="attached-world-2">
            <body name="attached-sphere-2">
              <joint type="slide"/>
              <geom size=".1"/>
            </body>
            <camera pos="0 0 0" quat="1 0 0 0"/>
          </frame>
        </frame>
      </body>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child1 = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child1, NotNull()) << er.data();
  mjSpec* child2 = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child2, NotNull()) << er.data();

  // attach a body to the frame
  mjsFrame* frame = mjs_findFrame(parent, "frame");
  EXPECT_THAT(frame, NotNull());
  mjsBody* body = mjs_findBody(child1, "sphere");
  EXPECT_THAT(body, NotNull());
  mjsBody* attached =
      mjs_asBody(mjs_attach(frame->element, body->element, "attached-", "-1"));
  EXPECT_THAT(attached, NotNull());
  mjModel* model1 = mj_compile(parent, 0);
  EXPECT_THAT(model1, NotNull());

  // attach the world to the same frame and convert it to a frame
  mjsBody* world = mjs_findBody(child2, "world");
  EXPECT_THAT(world, NotNull());
  mjsBody* child_world =
      mjs_asBody(mjs_attach(frame->element, world->element, "attached-", "-2"));
  EXPECT_THAT(child_world, NotNull());
  mjsFrame* frame_world = mjs_bodyToFrame(&child_world);
  EXPECT_THAT(frame_world, NotNull());
  EXPECT_THAT(child_world, IsNull());

  // compile and compare
  mjModel* model2 = mj_compile(parent, 0);
  EXPECT_THAT(model2, NotNull());
  mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model2, expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  mj_deleteSpec(parent);
  mj_deleteSpec(child1);
  mj_deleteSpec(child2);
  mj_deleteModel(model1);
  mj_deleteModel(model2);
  mj_deleteModel(expected);
}

TEST_F(MujocoTest, BodyToFrameWithInertial) {
  static constexpr char xml_child[] = R"(
    <mujoco>
      <worldbody>
        <body name="parent">
          <body name="child">
            <inertial mass="1" pos="0 0 0" quat="1 0 0 0" diaginertia="1 2 3"/>
          </body>
        </body>
      </worldbody>
    </mujoco>)";

  std::array<char, 1000> er;
  mjSpec* spec = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(spec, NotNull()) << er.data();
  mjModel* model = mj_compile(spec, 0);
  EXPECT_THAT(model, NotNull());
  mjsBody* parent = mjs_findBody(spec, "parent");
  EXPECT_THAT(parent, NotNull());
  mjsBody* child = mjs_findBody(spec, "child");
  EXPECT_THAT(child, NotNull());
  mjs_bodyToFrame(&child);
  EXPECT_THAT(parent->mass, 1);
  EXPECT_THAT(parent->fullinertia[0], 1);
  EXPECT_THAT(parent->fullinertia[1], 2);
  EXPECT_THAT(parent->fullinertia[2], 3);
  mj_deleteSpec(spec);
  mj_deleteModel(model);
}

TEST_F(MujocoTest, AttachSpecToSite) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <site name="site" pos="1 2 3"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="sphere">
        <joint type="slide"/>
        <geom size=".1"/>
      </body>
      <camera pos="0 0 0" quat="1 0 0 0"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <site name="site" pos="1 2 3"/>
        <frame name="attached-world-1" pos="1 2 3">
          <body name="attached-sphere-1">
            <joint type="slide"/>
            <geom size=".1"/>
          </body>
          <camera pos="0 0 0" quat="1 0 0 0"/>
        </frame>
      </body>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();
  mjsSite* site = mjs_asSite(mjs_findElement(parent, mjOBJ_SITE, "site"));
  EXPECT_THAT(site, NotNull());

  // add a frame to the child
  mjsBody* world = mjs_findBody(child, "world");
  EXPECT_THAT(world, NotNull());
  mjsFrame* frame = mjs_addFrame(world, 0);
  EXPECT_THAT(frame, NotNull());
  mjs_setName(frame->element, "world");
  mjs_setFrame(mjs_firstChild(world, mjOBJ_BODY, 0), frame);
  mjs_setFrame(mjs_firstChild(world, mjOBJ_CAMERA, 0), frame);

  // attach the entire spec to the site
  mjsFrame* worldframe =
      mjs_asFrame(mjs_attach(site->element, frame->element, "attached-", "-1"));
  EXPECT_THAT(worldframe, NotNull());

  // compile and compare
  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());
  mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  // check that the child world still exists
  mjsBody* child_world = mjs_findBody(child, "world");
  EXPECT_THAT(child_world, NotNull());

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(model);
  mj_deleteModel(expected);
}

TEST_F(MujocoTest, AttachSpecToBody) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <body name="body"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="sphere">
        <joint type="slide"/>
        <geom size=".1"/>
      </body>
      <camera pos="0 0 0" quat="1 0 0 0"/>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_result[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <frame name="attached-world-1" pos="1 2 3">
          <body name="attached-sphere-1">
            <joint type="slide"/>
            <geom size=".1"/>
          </body>
          <camera pos="0 0 0" quat="1 0 0 0"/>
        </frame>
      </body>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();
  mjsBody* body = mjs_findBody(parent, "body");
  EXPECT_THAT(body, NotNull());

  // add a frame to the child
  mjsBody* world = mjs_findBody(child, "world");
  EXPECT_THAT(world, NotNull());
  mjsFrame* frame = mjs_addFrame(world, 0);
  EXPECT_THAT(frame, NotNull());
  mjs_setName(frame->element, "world");
  mjs_setFrame(mjs_firstChild(world, mjOBJ_BODY, 0), frame);
  mjs_setFrame(mjs_firstChild(world, mjOBJ_CAMERA, 0), frame);

  // attach the entire spec to the site
  mjsFrame* worldframe =
      mjs_asFrame(mjs_attach(body->element, frame->element, "attached-", "-1"));
  EXPECT_THAT(worldframe, NotNull());
  worldframe->pos[0] = 1;
  worldframe->pos[1] = 2;
  worldframe->pos[2] = 3;

  // compile and compare
  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());
  mjModel* expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  // check that the child world still exists
  mjsBody* child_world = mjs_findBody(child, "world");
  EXPECT_THAT(child_world, NotNull());

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(model);
  mj_deleteModel(expected);
}

TEST_F(MujocoTest, PreserveState) {
  std::array<char, 1000> er;
  std::string field = "";

  static constexpr char xml_full[] = R"(
  <mujoco>
    <worldbody>
      <body name="detachable" pos="1 0 0">
        <joint type="hinge" axis="0 0 1" name="hinge"/>
        <geom type="sphere" size=".1"/>
      </body>
      <body name="persistent">
        <joint type="slide" axis="0 0 1" name="slide"/>
        <geom type="sphere" size=".2"/>
      </body>
      <body name="mocap_detach" mocap="true"/>
      <body name="mocap" mocap="true"/>
    </worldbody>
    <actuator>
      <position name="hinge" joint="hinge" timeconst=".01"/>
      <position name="slide" joint="slide" timeconst=".01"/>
    </actuator>
  </mujoco>)";

  static constexpr char xml_expected[] = R"(
  <mujoco>
    <worldbody>
      <body name="persistent">
        <joint type="slide" axis="0 0 1" name="slide"/>
        <geom type="sphere" size=".2"/>
      </body>
      <body name="newbody" pos="2 0 0">
        <joint type="slide" axis="0 0 1"/>
        <geom type="sphere" size=".3"/>
      </body>
      <body name="mocap" mocap="true"/>
    </worldbody>
    <actuator>
      <position name="slide" joint="slide" timeconst=".01"/>
    </actuator>
  </mujoco>)";

  // load spec
  mjSpec* spec = mj_parseXMLString(xml_full, 0, er.data(), er.size());
  EXPECT_THAT(spec, NotNull()) << er.data();

  // compile models
  mjModel* model = mj_compile(spec, 0);
  EXPECT_THAT(model, NotNull());
  mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull());

  // create data
  mjData* data = mj_makeData(model);
  EXPECT_THAT(data, NotNull());
  mjData* d_expected = mj_makeData(m_expected);
  EXPECT_THAT(d_expected, NotNull());

  // set ctrl
  data->ctrl[0] = 1;
  data->ctrl[1] = 2;
  d_expected->ctrl[0] = 2;

  // set mocap
  data->mocap_pos[3] = 1;
  data->mocap_quat[4] = 0;
  data->mocap_quat[5] = 1;
  d_expected->mocap_pos[0] = 1;
  d_expected->mocap_quat[0] = 0;
  d_expected->mocap_quat[1] = 1;

  // step models
  mj_step(model, data);
  mj_step(m_expected, d_expected);
  EXPECT_THAT(data->time, model->opt.timestep);

  // detach subtree
  mjsBody* body = mjs_findBody(spec, "detachable");
  EXPECT_THAT(body, NotNull());
  EXPECT_THAT(mjs_delete(spec, body->element), 0);

  // detach mocap
  mjsBody* mocap_body = mjs_findBody(spec, "mocap_detach");
  EXPECT_THAT(mocap_body, NotNull());
  EXPECT_THAT(mjs_delete(spec, mocap_body->element), 0);

  // add body
  mjsBody* newbody = mjs_addBody(mjs_findBody(spec, "world"), 0);
  EXPECT_THAT(newbody, NotNull());

  // add geom and joint
  mjsGeom* geom = mjs_addGeom(newbody, 0);
  mjsJoint* joint = mjs_addJoint(newbody, 0);

  // set properties
  newbody->pos[0] = 2;
  geom->size[0] = .3;
  joint->type = mjJNT_SLIDE;
  joint->axis[0] = 0;
  joint->axis[1] = 0;
  joint->axis[2] = 1;
  joint->ref = d_expected->qpos[m_expected->nq-1];

  // compile new model
  mj_recompile(spec, 0, model, data);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(data->time, model->opt.timestep);

  // compare qpos
  EXPECT_EQ(model->nq, m_expected->nq);
  for (int i = 0; i < model->nq; ++i) {
    EXPECT_EQ(data->qpos[i], d_expected->qpos[i]) << i;
  }

  // compare qvel
  EXPECT_EQ(model->nv, m_expected->nv);
  for (int i = 0; i < model->nv-1; ++i) {
    EXPECT_EQ(data->qvel[i], d_expected->qvel[i]) << i;
  }

  // second body was added after stepping so qvel should be zero
  EXPECT_EQ(data->qvel[model->nv-1], 0);

  // compare act
  EXPECT_EQ(model->na, m_expected->na);
  for (int i = 0; i < model->na; ++i) {
    EXPECT_EQ(data->act[i], d_expected->act[i]) << i;
  }

  // compare mocap
  EXPECT_EQ(model->nmocap, m_expected->nmocap);
  for (int i = 0; i < model->nmocap; ++i) {
    for (int j = 0; j < 3; ++j) {
      EXPECT_EQ(data->mocap_pos[3*i+j], d_expected->mocap_pos[3*i+j]) << i;
    }
    for (int j = 0; j < 4; ++j) {
      EXPECT_EQ(data->mocap_quat[4*i+j], d_expected->mocap_quat[4*i+j]) << i;
    }
  }

  // check that the function is callable with no data
  mj_deleteData(data);
  mj_recompile(spec, 0, model, nullptr);

  // destroy everything
  mj_deleteData(d_expected);
  mj_deleteSpec(spec);
  mj_deleteModel(model);
  mj_deleteModel(m_expected);
}

TEST_F(MujocoTest, RecompileAttach) {
  std::array<char, 1000> er;
  std::string field = "";

  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint type="slide" axis="0 0 1"/>
        <geom size=".2"/>
      </body>
    </worldbody>
  </mujoco>)";

  mjSpec* parent = mj_makeSpec();
  EXPECT_THAT(parent, NotNull());

  mjSpec* child1 = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(child1, NotNull());
  mjSpec* child2 = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(child2, NotNull());

  mjsElement* frame1 = mjs_addFrame(mjs_findBody(parent, "world"), 0)->element;
  mjs_attach(frame1, mjs_findBody(child1, "body")->element, "child-", "-1");

  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nq, 1);
  mjData* data = mj_makeData(model);
  EXPECT_THAT(data, NotNull());

  for (int i = 0; i < 100; i++) {
    mj_step(model, data);
  }

  mjsElement* frame2 = mjs_addFrame(mjs_findBody(parent, "world"), 0)->element;
  mjs_attach(frame2, mjs_findBody(child2, "body")->element, "child-", "-2");

  EXPECT_EQ(mj_recompile(parent, 0, model, data), 0);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nq, 2);
  EXPECT_NE(data->qpos[0], data->qpos[1]);
  EXPECT_EQ(data->qpos[1], 0);

  mj_deleteData(data);
  mj_deleteModel(model);
  mj_deleteSpec(child1);
  mj_deleteSpec(child2);
  mj_deleteSpec(parent);
}

TEST_F(MujocoTest, AttachMocap) {
  std::array<char, 1000> er;
  mjtNum tol = 0;
  std::string field = "";

  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body pos="1 1 1" quat="0 1 0 0" name="mocap" mocap="true"/>
    </worldbody>
    <keyframe>
      <key name="key" time="1" mpos="2 2 2" mquat="0 0 0 1"/>
    </keyframe>
  </mujoco>)";

  static constexpr char xml_expected[] = R"(
  <mujoco>
    <worldbody>
      <body pos="1 1 1" quat="0 1 0 0" name="mocap" mocap="true"/>
      <body pos="1 1 1" quat="0 1 0 0" name="attached-mocap-1" mocap="true"/>
    </worldbody>
    <keyframe>
      <key name="key" time="1" mpos="2 2 2 1 1 1" mquat="0 0 0 1 0 1 0 0"/>
      <key name="attached-key-1" time="1" mpos="1 1 1 2 2 2" mquat="0 1 0 0 0 0 0 1"/>
    </keyframe>
  </mujoco>)";

  mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(spec, NotNull()) << er.data();
  mjs_setDeepCopy(spec, true);  // needed for self-attach

  mjsBody* body = mjs_findBody(spec, "mocap");
  EXPECT_THAT(body, NotNull());

  mjsBody* world = mjs_findBody(spec, "world");
  EXPECT_THAT(world, NotNull());

  mjsElement* frame = mjs_addFrame(world, NULL)->element;
  mjs_attach(frame, body->element, "attached-", "-1");

  mjsBody* attached_body = mjs_findBody(spec, "attached-mocap-1");
  EXPECT_THAT(attached_body, NotNull());

  mjModel* model = mj_compile(spec, 0);
  EXPECT_THAT(model, NotNull());

  mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, m_expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  mj_deleteSpec(spec);
  mj_deleteModel(model);
  mj_deleteModel(m_expected);
}

TEST_F(MujocoTest, ReplicateKeyframe) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <replicate count="1" euler="0 0 1.8">
        <body name="body" pos="0 -1 0">
          <joint type="slide"/>
          <geom name="g" size="1"/>
        </body>
      </replicate>
    </worldbody>

    <keyframe>
      <key name="keyframe" qpos="1"/>
    </keyframe>
  </mujoco>

  )";
  std::array<char, 1024> error;
  mjModel* m = LoadModelFromString(xml, error.data(), error.size());
  EXPECT_THAT(m, testing::NotNull()) << error.data();
  EXPECT_THAT(m->ngeom, 1);
  EXPECT_THAT(m->nbody, 2);

  // check that the keyframe is resized
  EXPECT_THAT(m->nkey, 2);
  EXPECT_THAT(m->nq, 1);
  EXPECT_THAT(m->key_qpos[0], 1);
  EXPECT_THAT(m->key_qpos[1], 0);
  EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 0), "keyframe0");
  EXPECT_STREQ(mj_id2name(m, mjOBJ_KEY, 1), "keyframe");

  mj_deleteModel(m);
}

TEST_F(MujocoTest, AttachUnnamedAssets) {
  static constexpr char cube[] = R"(
  v -1 -1  1
  v  1 -1  1
  v -1  1  1
  v  1  1  1
  v -1  1 -1
  v  1  1 -1
  v -1 -1 -1
  v  1 -1 -1)";

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));

  mjSpec* child = mj_makeSpec();
  mjsMesh* mesh = mjs_addMesh(child, 0);
  mjsFrame* frame = mjs_addFrame(mjs_findBody(child, "world"), 0);
  mjsGeom* geom = mjs_addGeom(mjs_findBody(child, "world"), 0);
  mjs_setFrame(geom->element, frame);
  mjs_setString(mesh->file, "cube.obj");
  mjs_setString(geom->meshname, "cube");
  geom->type = mjGEOM_MESH;

  mjSpec* spec = mj_makeSpec();
  mjs_attach(mjs_findBody(spec, "world")->element, frame->element, "_", "");

  mjModel* model = mj_compile(spec, vfs.get());
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->nmesh, 1);
  EXPECT_STREQ(mj_id2name(model, mjOBJ_MESH, 0), "_cube");

  mj_deleteVFS(vfs.get());
  mj_deleteSpec(spec);
  mj_deleteSpec(child);
  mj_deleteModel(model);
}

TEST_F(MujocoTest, InitTexture) {
  mjSpec* spec = mj_makeSpec();
  EXPECT_THAT(spec, NotNull());

  mjsTexture* texture = mjs_addTexture(spec);
  mjs_setName(texture->element, "checker");
  texture->type = mjTEXTURE_CUBE;
  texture->builtin = mjBUILTIN_CHECKER;
  texture->width = 300;
  texture->height = 300;

  mjsMaterial* material = mjs_addMaterial(spec, 0);
  mjs_setName(material->element, "floor");
  mjs_setInStringVec(material->textures, mjTEXROLE_RGB, "checker");

  mjsGeom* floor = mjs_addGeom(mjs_findBody(spec, "world"), 0);
  mjs_setString(floor->material, "floor");
  floor->type = mjGEOM_PLANE;
  floor->size[0] = 1;
  floor->size[1] = 1;
  floor->size[2] = 0.01;
  mjs_setString(floor->material, "floor");

  mjModel* model = mj_compile(spec, 0);
  EXPECT_THAT(model, NotNull());

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

void AttachNestedKeyframe(bool compile) {
  static constexpr char parent_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint name="joint" type="slide"/>
        <geom size=".1"/>
      </body>
      <frame name="frame"/>
    </worldbody>
  </mujoco>)";

  static constexpr char child_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint name="joint" type="slide"/>
        <geom size=".2"/>
        <frame name="frame"/>
      </body>
    </worldbody>
    <keyframe>
      <key name="key2" time="2" qpos="2"/>
    </keyframe>
  </mujoco>)";

  static constexpr char gchild_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint name="joint" type="slide"/>
        <geom size=".3"/>
      </body>
    </worldbody>
    <keyframe>
      <key name="key1" time="1" qpos="1"/>
    </keyframe>
  </mujoco>)";

  static constexpr char expected_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint name="joint" type="slide"/>
        <geom size=".1"/>
      </body>
      <frame name="frame">
        <body name="child-body">
          <joint name="child-joint" type="slide"/>
          <geom size=".2"/>
          <frame name="child-frame">
            <body name="child-gchild-body">
              <joint name="child-gchild-joint" type="slide"/>
              <geom size=".3"/>
            </body>
          </frame>
        </body>
      </frame>
    </worldbody>
    <keyframe>
      <key name="child-key2" time="2" qpos="0 2 0"/>
      <key name="child-gchild-key1" time="1" qpos="0 0 1"/>
    </keyframe>
  </mujoco>)";

  static constexpr char expected_xml_uncompiled[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <joint name="joint" type="slide"/>
        <geom size=".1"/>
      </body>
      <frame name="frame">
        <body name="child-body">
          <joint name="child-joint" type="slide"/>
          <geom size=".2"/>
          <frame name="child-frame">
            <body name="child-gchild-body">
              <joint name="child-gchild-joint" type="slide"/>
              <geom size=".3"/>
            </body>
          </frame>
        </body>
      </frame>
    </worldbody>
    <keyframe>
      <key name="child-key2" time="2" qpos="0 2 0"/>
      <key name="gchild-key1" time="1" qpos="0 0 1"/>
    </keyframe>
  </mujoco>)";

  std::array<char, 1000> er;
  mjSpec* parent = mj_parseXMLString(parent_xml, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(child_xml, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();
  mjSpec* gchild = mj_parseXMLString(gchild_xml, 0, er.data(), er.size());
  EXPECT_THAT(gchild, NotNull()) << er.data();

  mjs_setDeepCopy(parent, true);
  mjs_setDeepCopy(child, true);

  // attach gchild to child
  mjs_attach(mjs_findFrame(child, "frame")->element,
             mjs_findBody(gchild, "body")->element, "gchild-", "");

  // compile required before further attachment
  mjModel* m_child = compile ? mj_compile(child, 0) : nullptr;

  // check warning is issued, empty for a compiled model
  static char warning[1024];
  warning[0] = '\0';
  mju_user_warning = [](const char* msg) {
      util::strcpy_arr(warning, msg);
  };

  // attach child to parent
  mjs_attach(mjs_findFrame(parent, "frame")->element,
             mjs_findBody(child, "body")->element, "child-", "");

  EXPECT_THAT(warning, HasSubstr(compile ? "" : "model has pending keyframes"));

  // compare models
  mjtNum tol = 0;
  std::string field = "";
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull());
  mjModel* m_expected = LoadModelFromString(
      compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
  EXPECT_THAT(m_expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';;

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteSpec(gchild);
  mj_deleteModel(m_expected);
  mj_deleteModel(m_attached);
  mj_deleteModel(m_child);
}

TEST_F(MujocoTest, TestAttachNestedKeyframe) {
  AttachNestedKeyframe(/*compile=*/true);
  AttachNestedKeyframe(/*compile=*/false);
}

TEST_F(MujocoTest, RepeatedAttachKeyframe) {
  static constexpr char xml_1[] = R"(
    <mujoco model="MuJoCo Model">
      <worldbody>
        <body name="body"/>
      </worldbody>
    </mujoco>)";

  static constexpr char xml_2[] = R"(
    <mujoco model="MuJoCo Model">
      <worldbody>
        <body name="b1">
          <joint/>
          <geom size="0.1"/>
        </body>
        <body name="b2">
          <joint/>
          <geom size="0.1"/>
        </body>
      </worldbody>
      <keyframe>
        <key name="home" qpos="1 2" />
      </keyframe>
    </mujoco>)";

    std::array<char, 1000> er;
    mjSpec* parent = mj_parseXMLString(xml_1, 0, er.data(), er.size());
    EXPECT_THAT(parent, NotNull()) << er.data();
    mjSpec* child = mj_parseXMLString(xml_2, 0, er.data(), er.size());
    EXPECT_THAT(child, NotNull()) << er.data();

    mjsBody* body_1 = mjs_findBody(parent, "body");
    mjsElement* attachment_frame = mjs_addFrame(body_1, 0)->element;
    mjs_attach(attachment_frame, mjs_findBody(child, "b1")->element, "b1-", "");
    mjModel* model_1 = mj_compile(parent, 0);
    EXPECT_THAT(model_1, NotNull());
    mjs_attach(attachment_frame, mjs_findBody(child, "b2")->element, "b2-", "");
    mjModel* model_2 = mj_compile(parent, 0);
    EXPECT_THAT(model_2, NotNull());

    EXPECT_EQ(model_1->nkey, 1);
    EXPECT_EQ(model_2->nkey, 2);
    EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 0), "b1-home");
    EXPECT_STREQ(mj_id2name(model_2, mjOBJ_KEY, 1), "b2-home");

    mj_deleteSpec(parent);
    mj_deleteSpec(child);
    mj_deleteModel(model_1);
    mj_deleteModel(model_2);
}

TEST_F(MujocoTest, ResizeParentKeyframe) {
  static constexpr char xml_parent[] = R"(
    <mujoco model="MuJoCo Model">
      <worldbody>
        <frame name="frame"/>
        <body name="body">
          <joint/>
          <geom size="0.1"/>
        </body>
      </worldbody>
      <keyframe>
        <key name="home" qpos="1"/>
      </keyframe>
    </mujoco>)";

  static constexpr char xml_child[] = R"(
    <mujoco model="MuJoCo Model">
      <worldbody>
        <body name="body">
          <joint/>
          <geom size="0.1"/>
        </body>
      </worldbody>
    </mujoco>)";

  static constexpr char xml_expected[] = R"(
    <mujoco model="MuJoCo Model">
      <worldbody>
        <body name="body">
          <joint/>
          <geom size="0.1"/>
        </body>
        <frame name="frame">
          <body name="child-body">
            <joint/>
            <geom size="0.1"/>
          </body>
        </frame>
      </worldbody>
      <keyframe>
        <key name="home" qpos="1 0"/>
      </keyframe>
    </mujoco>)";

  std::array<char, 1000> er;
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, er.data(), er.size());
  EXPECT_THAT(parent, NotNull()) << er.data();
  mjSpec* child = mj_parseXMLString(xml_child, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();

  mjs_attach(mjs_findFrame(parent, "frame")->element,
             mjs_findBody(child, "body")->element, "child-", "");

  mjModel* model = mj_compile(parent, 0);
  EXPECT_THAT(model, NotNull());

  mjtNum tol = 0;
  std::string field = "";
  mjModel* expected = LoadModelFromString(xml_expected, er.data(), er.size());
  EXPECT_THAT(expected, NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected, field), tol)
            << "Expected and attached models are different!\n"
            << "Different field: " << field << '\n';

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteModel(model);
  mj_deleteModel(expected);
}

TEST_F(MujocoTest, KeyframeSizeError) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <replicate count="2" offset="0 1 0">
        <body name="ball" pos="0 0 1">
          <geom type="sphere" size="0.1"/>
          <joint/>
        </body>
      </replicate>
    </worldbody>

    <keyframe>
      <key name="valid_qpos" qpos="0.5"/>
      <key name="invalid_qpos" qpos="0.5 0.25"/>
    </keyframe>
  </mujoco>
  )";

  std::array<char, 1000> er;
  mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(spec, IsNull());
  EXPECT_THAT(er.data(), HasSubstr(
      "Keyframe 'invalid_qpos' has invalid qpos size, got 2, should be 1"));
}

TEST_F(MujocoTest, DifferentUnitsAllowed) {
  static constexpr char gchild_xml[] = R"(
  <mujoco>
    <compiler angle="degree"/>

    <worldbody>
      <body name="gchild" euler="-90 0 0">
        <geom type="box" size="1 1 1"/>
        <joint name="gchild_joint" range="-180 180"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  static constexpr char child_xml[] = R"(
  <mujoco>
    <compiler angle="radian"/>

    <worldbody>
      <body name="child" euler="-1.5707963 0 0">
        <geom type="box" size="1 1 1"/>
        <joint name="child_joint" range="-3.1415926 3.1415926"/>
        <frame name="frame" euler="1.5707963 0 0"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  static constexpr char parent_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent">
        <geom type="box" size="1 1 1"/>
        <joint name="parent_joint" range="-180 180"/>
        <frame name="frame" euler="90 0 0"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  std::array<char, 1024> error;
  mjSpec* gchild =
      mj_parseXMLString(gchild_xml, 0, error.data(), error.size());
  mjSpec* child = mj_parseXMLString(child_xml, 0, error.data(), error.size());
  mjSpec* spec = mj_parseXMLString(parent_xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();
  mjs_attach(mjs_findFrame(child, "frame")->element,
             mjs_findBody(gchild, "gchild")->element, "gchild_", "");
  mjs_attach(mjs_findFrame(spec, "frame")->element,
             mjs_findBody(child, "child")->element, "child_", "");

  mjModel* model = mj_compile(spec, 0);
  EXPECT_THAT(model, NotNull());
  EXPECT_THAT(model->njnt, 3);
  EXPECT_NEAR(model->jnt_range[0], -mjPI, 1e-6);
  EXPECT_NEAR(model->jnt_range[1], mjPI, 1e-6);
  EXPECT_NEAR(model->jnt_range[2], -mjPI, 1e-6);
  EXPECT_NEAR(model->jnt_range[3], mjPI, 1e-6);
  EXPECT_NEAR(model->jnt_range[4], -mjPI, 1e-6);
  EXPECT_NEAR(model->jnt_range[5], mjPI, 1e-6);
  EXPECT_NEAR(model->body_quat[4], 1, 1e-12);
  EXPECT_NEAR(model->body_quat[5], 0, 1e-12);
  EXPECT_NEAR(model->body_quat[6], 0, 1e-12);
  EXPECT_NEAR(model->body_quat[7], 0, 1e-12);

  mjSpec* copied_spec = mj_copySpec(spec);
  ASSERT_THAT(copied_spec, NotNull());
  mj_deleteSpec(gchild);
  mj_deleteSpec(child);
  mj_deleteSpec(spec);
  mj_deleteModel(model);

  // check that deleting `parent` or `child` does not invalidate the copy
  mjModel* copied_model = mj_compile(copied_spec, 0);
  EXPECT_THAT(copied_model, NotNull());
  EXPECT_THAT(copied_model->njnt, 3);
  EXPECT_NEAR(copied_model->jnt_range[0], -mjPI, 1e-6);
  EXPECT_NEAR(copied_model->jnt_range[1], mjPI, 1e-6);
  EXPECT_NEAR(copied_model->jnt_range[2], -mjPI, 1e-6);
  EXPECT_NEAR(copied_model->jnt_range[3], mjPI, 1e-6);
  EXPECT_NEAR(copied_model->jnt_range[4], -mjPI, 1e-6);
  EXPECT_NEAR(copied_model->jnt_range[5], mjPI, 1e-6);
  EXPECT_NEAR(copied_model->body_quat[4], 1, 1e-12);
  EXPECT_NEAR(copied_model->body_quat[5], 0, 1e-12);
  EXPECT_NEAR(copied_model->body_quat[6], 0, 1e-12);
  EXPECT_NEAR(copied_model->body_quat[7], 0, 1e-12);

  mj_deleteSpec(copied_spec);
  mj_deleteModel(copied_model);
}

TEST_F(MujocoTest, DifferentOptionsInAttachedFrame) {
  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody/>
  </mujoco>
  )";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <compiler eulerseq="zyx"/>
    <worldbody>
      <frame name="child" >
        <site euler="0 90 180"/>
      </frame>
    </worldbody>
  </mujoco>
  )";

  // load specs and compile child
  mjSpec* parent = mj_parseXMLString(xml_parent, 0, nullptr, 0);
  EXPECT_THAT(parent, NotNull());
  mjSpec* child1 = mj_parseXMLString(xml_child, 0, nullptr, 0);
  EXPECT_THAT(child1, NotNull());
  mjModel* m_child1 = mj_compile(child1, 0);
  EXPECT_THAT(m_child1, NotNull());
  mjSpec* child2 = mj_parseXMLString(xml_child, 0, nullptr, 0);
  EXPECT_THAT(child2, NotNull());
  mjModel* m_child2 = mj_compile(child1, 0);
  EXPECT_THAT(m_child2, NotNull());

  // attach child frame to parent worldbody
  mjsBody* world = mjs_findBody(parent, "world");
  EXPECT_THAT(world, NotNull());
  mjsFrame* child1_frame = mjs_findFrame(child1, "child");
  EXPECT_THAT(child1_frame, NotNull());
  mjsFrame* child2_frame = mjs_findFrame(child2, "child");
  EXPECT_THAT(child2_frame, NotNull());
  mjsElement* attached_frame1 =
      mjs_attach(world->element, child1_frame->element, "child-", "-1");
  EXPECT_THAT(attached_frame1, NotNull());
  mjsElement* attached_frame2 =
      mjs_attach(world->element, child2_frame->element, "child-", "-2");
  EXPECT_THAT(attached_frame2, NotNull());

  // wrap the child frame in the parent frame and compile
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull());
  EXPECT_NEAR(m_attached->site_quat[0], m_child1->site_quat[0], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[1], m_child1->site_quat[1], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[2], m_child1->site_quat[2], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[3], m_child1->site_quat[3], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[4], m_child2->site_quat[0], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[5], m_child2->site_quat[1], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[6], m_child2->site_quat[2], 1e-6);
  EXPECT_NEAR(m_attached->site_quat[7], m_child2->site_quat[3], 1e-6);

  mj_deleteSpec(parent);
  mj_deleteSpec(child1);
  mj_deleteModel(m_child1);
  mj_deleteSpec(child2);
  mj_deleteModel(m_child2);
  mj_deleteModel(m_attached);
}

TEST_F(MujocoTest, NotCopyAttachedSpec) {
  static constexpr char xml_parent[] = R"(
  <mujoco>
    <asset>
      <model name="child" file="xml_child.xml"/>
    </asset>
    <worldbody>
      <body name="parent">
        <geom name="geom" size="2"/>
        <attach model="child" body="body" prefix="other"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <geom name="geom" size="1" pos="2 0 0"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "xml_child.xml", xml_child, sizeof(xml_child));

  std::array<char, 1024> er;
  mjSpec* spec = mj_parseXMLString(xml_parent, vfs.get(), er.data(), er.size());
  EXPECT_THAT(spec, NotNull()) << er.data();

  mjModel* model = mj_compile(spec, vfs.get());
  EXPECT_THAT(model, NotNull()) << er.data();

  mjSpec* child = mjs_findSpec(spec, "child");
  EXPECT_THAT(child, NotNull());

  mjSpec* copy = mj_copySpec(spec);
  EXPECT_THAT(copy, NotNull());

  mjSpec* child_copy = mjs_findSpec(copy, "child");
  EXPECT_THAT(child_copy, NotNull());
  EXPECT_EQ(child_copy, child);

  mj_deleteSpec(spec);
  mj_deleteSpec(copy);
  mj_deleteModel(model);
  mj_deleteVFS(vfs.get());
}

TEST_F(MujocoTest, ApplyNameSpaceToDefaults) {
  static constexpr char xml_c[] = R"(
  <mujoco>
    <default>
      <default class="mesh">
        <mesh scale="0.001 0.001 0.001"/>
      </default>
    </default>
    <asset>
      <mesh file="cube.obj" class="mesh"/>
    </asset>
    <worldbody>
      <body name="body">
        <geom type="mesh" mesh="cube"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char xml_p[] = R"(
  <mujoco>
    <worldbody>
      <frame name="parent"/>
    </worldbody>
  </mujoco>
  )";

  static constexpr char cube[] = R"(
  v -0.500000 -0.500000  0.500000
  v  0.500000 -0.500000  0.500000
  v -0.500000  0.500000  0.500000
  v  0.500000  0.500000  0.500000
  v -0.500000  0.500000 -0.500000
  v  0.500000  0.500000 -0.500000
  v -0.500000 -0.500000 -0.500000
  v  0.500000 -0.500000 -0.500000)";

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));

  std::array<char, 1024> err;
  mjSpec* child = mj_parseXMLString(xml_c, vfs.get(), err.data(), err.size());
  EXPECT_THAT(child, NotNull()) << err.data();
  mjSpec* parent = mj_parseXMLString(xml_p, 0, err.data(), err.size());
  EXPECT_THAT(parent, NotNull()) << err.data();

  mjsElement* attached =
      mjs_attach(mjs_findFrame(parent, "parent")->element,
                 mjs_findBody(child, "body")->element, "child-", "");
  EXPECT_THAT(attached, NotNull());

  mjModel* model = mj_compile(parent, vfs.get());
  EXPECT_THAT(model, NotNull());

  mj_deleteSpec(child);
  mj_deleteSpec(parent);
  mj_deleteModel(model);
  mj_deleteVFS(vfs.get());
}

TEST_F(MujocoTest, DetachDefault) {
  static constexpr char xml_c[] = R"(
  <mujoco>
    <default>
      <default class="parent">
        <default class="child1">
          <mesh scale="0.001 0.001 0.001"/>
        </default>
        <default class="child2">
          <mesh scale="0.001 0.001 0.001"/>
        </default>
      </default>
    </default>
    <asset>
      <mesh file="cube.obj" class="child2"/>
    </asset>
    <worldbody>
      <body name="body">
        <geom type="mesh" mesh="cube"/>
      </body>
    </worldbody>
  </mujoco>)";

  static constexpr char cube[] = R"(
  v -0.500000 -0.500000  0.500000
  v  0.500000 -0.500000  0.500000
  v -0.500000  0.500000  0.500000
  v  0.500000  0.500000  0.500000
  v -0.500000  0.500000 -0.500000
  v  0.500000  0.500000 -0.500000
  v -0.500000 -0.500000 -0.500000
  v  0.500000 -0.500000 -0.500000)";

  auto vfs = std::make_unique<mjVFS>();
  mj_defaultVFS(vfs.get());
  mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube));

  std::array<char, 1024> err;
  mjSpec* spec = mj_parseXMLString(xml_c, vfs.get(), err.data(), err.size());
  EXPECT_THAT(spec, NotNull()) << err.data();

  // get default
  mjsDefault* child = mjs_findDefault(spec, "child1");
  EXPECT_THAT(child, NotNull());

  // delete default
  EXPECT_EQ(mjs_delete(spec, child->element), 0);
  child = mjs_findDefault(spec, "child1");
  EXPECT_THAT(child, IsNull());

  // try and detach previously detached default, should fail
  EXPECT_EQ(mjs_delete(spec, nullptr), -1);
  child = mjs_findDefault(spec, "child1");
  EXPECT_THAT(child, IsNull());

  // detach parent
  mjsDefault* parent = mjs_findDefault(spec, "parent");
  EXPECT_THAT(parent, NotNull());
  mjs_delete(spec, parent->element);

  // both parent and remaining child should be removed
  parent = mjs_findDefault(spec, "parent");
  EXPECT_THAT(parent, IsNull());
  child = mjs_findDefault(spec, "child2");
  EXPECT_THAT(child, IsNull());

  // error when trying to detach the 'main' default
  mjsDefault* main = mjs_findDefault(spec, "main");
  EXPECT_THAT(main, NotNull());
  EXPECT_EQ(mjs_delete(spec, main->element), -1);
  EXPECT_THAT(mjs_getError(spec),
              HasSubstr("cannot remove the global default ('main')"));

  main = mjs_findDefault(spec, "main");
  EXPECT_THAT(main, NotNull());

  mj_deleteVFS(vfs.get());
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, ErrorWhenCompilingOrphanedSpec) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="a"/>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> er;
  mjSpec* child = mj_parseXMLString(xml, 0, er.data(), er.size());
  EXPECT_THAT(child, NotNull()) << er.data();
  mjSpec* parent = mj_makeSpec();
  EXPECT_THAT(parent, NotNull());
  mjsBody* body = mjs_findBody(child, "a");
  EXPECT_THAT(body, NotNull());
  mjsFrame* frame = mjs_addFrame(mjs_findBody(parent, "world"), nullptr);
  EXPECT_THAT(frame, NotNull());
  mjs_attach(frame->element, body->element, "child-", "");
  mj_deleteSpec(parent);
  mjModel* model = mj_compile(child, 0);
  EXPECT_THAT(model, IsNull());
  EXPECT_THAT(mjs_getError(child), HasSubstr("by reference to a parent"));
  mj_deleteSpec(child);
}

TEST_F(MujocoTest, SetFrameReverseOrder) {
  mjSpec* spec = mj_makeSpec();
  mjsBody* world = mjs_findBody(spec, "world");
  mjsFrame* child = mjs_addFrame(world, nullptr);
  mjsFrame* parent = mjs_addFrame(world, nullptr);
  mjs_setName(child->element, "child");
  mjs_setName(parent->element, "parent");
  mjs_setFrame(child->element, parent);
  mjSpec* copy = mj_copySpec(spec);
  EXPECT_THAT(copy, NotNull());
  EXPECT_THAT(mjs_findFrame(copy, "child"), NotNull());
  EXPECT_THAT(mjs_findFrame(copy, "parent"), NotNull());
  mj_deleteSpec(spec);
  mj_deleteSpec(copy);
}

TEST_F(MujocoTest, UserValue) {
  mjSpec* spec = mj_makeSpec();
  EXPECT_THAT(spec, NotNull());
  mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), nullptr);
  EXPECT_THAT(body, NotNull());
  std::string data = "data";
  mjs_setUserValue(body->element, "key", data.data());
  EXPECT_THAT(mjs_getUserValue(body->element, "invalid_key"), IsNull());
  const void* payload = mjs_getUserValue(body->element, "key");
  EXPECT_STREQ(static_cast<const char*>(payload), data.c_str());
  mjs_deleteUserValue(body->element, "key");
  EXPECT_THAT(mjs_getUserValue(body->element, "key"), IsNull());

  std::string* heap_data = new std::string("heap_data");
  mjs_setUserValueWithCleanup(
      body->element, "key", heap_data,
      [](const void* data) { delete static_cast<const std::string*>(data); });
  payload = mjs_getUserValue(body->element, "key");
  EXPECT_STREQ(static_cast<const std::string*>(payload)->c_str(),
               heap_data->c_str());
  mj_deleteSpec(spec);
}

}  // namespace
}  // namespace mujoco