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 <array>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <filesystem>  // NOLINT
#include <functional>
#include <map>
#include <memory>
#include <string>
#include <vector>

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <absl/strings/str_format.h>
#include <mujoco/mjplugin.h>
#include <mujoco/mjspec.h>
#include <mujoco/mujoco.h>
#include "src/xml/xml_api.h"
#include "test/compare_model.h"
#include "test/fixture.h"

namespace mujoco {
namespace {

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

// -------------------------- 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, AttachAndChildDeletion) {
  mjSpec* child_spec = mj_makeSpec();
  mjsBody* child_world = mjs_findBody(child_spec, "world");
  mjsBody* child_body = mjs_addBody(child_world, 0);
  mjsJoint* freejoint = mjs_addJoint(child_body, 0);
  freejoint->type = mjJNT_FREE;
  mjs_setName(freejoint->element, "child_freejoint");

  mjSpec* parent_spec = mj_makeSpec();
  mjsBody* parent_world = mjs_findBody(parent_spec, "world");
  mjsBody* parent_body = mjs_addBody(parent_world, 0);

  // Attach child spec to parent_body
  mjsElement* attached =
      mjs_attach(parent_body->element, child_spec->element, "pre_", "");
  ASSERT_THAT(attached, NotNull());

  // Delete freejoint from child_spec, should fail because it is attached
  int result = mjs_delete(child_spec, freejoint->element);
  EXPECT_EQ(result, -1);

  // The freejoint should still be in parent_spec because deletion failed
  mjsElement* found_joint =
      mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint");
  EXPECT_THAT(found_joint, NotNull());

  mj_deleteSpec(child_spec);
  mj_deleteSpec(parent_spec);
}

TEST_F(MujocoTest, OriginSpecInvariantToAttachment) {
  mjSpec* child_spec = mj_makeSpec();
  mjsBody* child_world = mjs_findBody(child_spec, "world");
  mjsBody* child_body = mjs_addBody(child_world, 0);
  mjsJoint* freejoint = mjs_addJoint(child_body, 0);
  freejoint->type = mjJNT_FREE;
  mjs_setName(freejoint->element, "child_freejoint");

  mjSpec* parent_spec = mj_makeSpec();
  mjsBody* parent_world = mjs_findBody(parent_spec, "world");
  mjsBody* parent_body = mjs_addBody(parent_world, 0);
  mjs_setName(parent_body->element, "parent_body");

  // Attach child spec to parent_body
  mjsElement* attached =
      mjs_attach(parent_body->element, child_spec->element, "pre_", "");
  ASSERT_THAT(attached, NotNull());

  // The freejoint should still be in parent_spec because deletion failed
  mjsElement* child_spec_joint =
      mjs_findElement(parent_spec, mjOBJ_JOINT, "pre_child_freejoint");
  EXPECT_EQ(mjs_getSpec(child_spec_joint), parent_spec);
  EXPECT_EQ(mjs_getOriginSpec(child_spec_joint), child_spec);

  mjsElement* parent_spec_body =
      mjs_findElement(parent_spec, mjOBJ_BODY, "parent_body");
  EXPECT_EQ(mjs_getOriginSpec(parent_spec_body), parent_spec);

  mj_deleteSpec(child_spec);
  mj_deleteSpec(parent_spec);
}

int open_mock(mjResource* resource) {
  static const char parent_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent_body"/>
    </worldbody>
  </mujoco>
  )";
  resource->data = mju_malloc(sizeof(parent_xml));
  absl::SNPrintF(static_cast<char*>(resource->data), sizeof(parent_xml), "%s",
                 parent_xml);
  return 1;
}

int read_mock(mjResource* resource, const void** buffer) {
  *buffer = resource->data;
  return std::strlen((const char*)resource->data);
}

void close_mock(mjResource* resource) {
  mju_free(resource->data);
  resource->data = nullptr;
}

TEST_F(MujocoTest, AttachedSpecDoesNotInheritURI) {
  // This test checks that when we attach a child spec to a parent spec that was
  // loaded from a resource provider, the child spec does not inherit the
  // resource URI from the parent. This allows the child spec to specify assets
  // relative to its model file or in the VFS.
  mjpResourceProvider provider = {
      .prefix = "fakeprovider",
      .open = open_mock,
      .read = read_mock,
      .close = close_mock,
  };

  mjp_registerResourceProvider(&provider);

  std::array<char, 1024> err;
  mjSpec* parent_spec =
      mj_parseXML("fakeprovider:parent.xml", nullptr, err.data(), err.size());
  mjs_setString(parent_spec->modelname, "parent");
  ASSERT_THAT(parent_spec, NotNull()) << err.data();

  // Create child spec
  static constexpr char child_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="child_body">
        <geom type="mesh" mesh="asset"/>
      </body>
    </worldbody>
    <asset>
      <mesh name="asset" file="asset.obj"/>
    </asset>
  </mujoco>
  )";

  // Setup VFS with asset
  mjVFS vfs;
  mj_defaultVFS(&vfs);
  static constexpr char asset_data[] = R"(
  v 0 0 0
  v 1 0 0
  v 0 1 0
  v 0 0 1
  f 1 2 3
  f 1 2 4
  f 2 3 4
  f 3 1 4
  )";
  mj_addBufferVFS(&vfs, "asset.obj", asset_data, sizeof(asset_data));

  mjSpec* child_spec =
      mj_parseXMLString(child_xml, &vfs, err.data(), err.size());
  mjs_setString(child_spec->modelname, "child");
  ASSERT_THAT(child_spec, NotNull()) << err.data();

  // Attach child spec to parent spec's world body
  mjsBody* world = mjs_findBody(parent_spec, "world");
  ASSERT_THAT(world, NotNull());

  mjsElement* attached =
      mjs_attach(world->element, child_spec->element, "", "");
  ASSERT_THAT(attached, NotNull());

  mjModel* model = mj_compile(parent_spec, &vfs);
  mj_deleteVFS(&vfs);

  EXPECT_THAT(model, NotNull()) << mjs_getError(parent_spec);

  if (model) {
    mj_deleteModel(model);
  }
  mj_deleteSpec(parent_spec);
  mj_deleteSpec(child_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);
}

TEST_F(MujocoTest, SetToDCMotorNullable) {
  mjSpec* spec = mj_makeSpec();
  mjsActuator* actuator = mjs_addActuator(spec, 0);

  double motorconst[2] = {0.05, 0.05};
  double resistance = 2.0;

  const char* err =
      mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
                       nullptr, nullptr, nullptr, nullptr, nullptr, 0);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->gainprm[0], 2.0);
  EXPECT_EQ(actuator->gainprm[1], 0.05);
  EXPECT_EQ(actuator->gainprm[4], 0);
  EXPECT_EQ(actuator->gainprm[5], 0);
  EXPECT_EQ(actuator->gainprm[6], 0);
  EXPECT_EQ(actuator->dynprm[7], 0);
  EXPECT_EQ(actuator->dynprm[8], 0);

  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, SetToDCMotorDeriveKe) {
  mjSpec* spec = mj_makeSpec();
  mjsActuator* actuator = mjs_addActuator(spec, 0);

  double resistance = 2.0;
  double nominal[3] = {12.0, 0, 100.0};  // vn=12, omega0=100

  const char* err =
      mjs_setToDCMotor(actuator, nullptr, resistance, nominal, nullptr, nullptr,
                       nullptr, nullptr, nullptr, nullptr, 0);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->gainprm[0], 2.0);
  EXPECT_NEAR(actuator->gainprm[1], 0.12, 1e-5);

  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, SetToDCMotorFull) {
  mjSpec* spec = mj_makeSpec();
  mjsActuator* actuator = mjs_addActuator(spec, 0);

  double motorconst[2] = {0.05, 0.05};
  double resistance = 2.0;
  double saturation[3] = {1.0, 2.0, 3.0};
  double controller[6] = {10.0, 20.0, 30.0, 40.0, 50.0, 60.0};

  const char* err =
      mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, saturation,
                       nullptr, nullptr, controller, nullptr, nullptr, 0);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->gainprm[0], 2.0);   // resistance
  EXPECT_EQ(actuator->gainprm[1], 0.05);  // K
  EXPECT_EQ(actuator->gainprm[4], 10.0);  // kp
  EXPECT_EQ(actuator->gainprm[5], 20.0);  // ki
  EXPECT_EQ(actuator->gainprm[6], 30.0);  // kd
  EXPECT_EQ(actuator->dynprm[7], 40.0);   // slewmax
  EXPECT_EQ(actuator->dynprm[8], 50.0);   // Imax
  EXPECT_EQ(actuator->gainprm[7], 60.0);  // Vmax
  EXPECT_EQ(actuator->dynprm[1], 3.0);    // (di/dt)_max

  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, SetToDCMotorLuGre) {
  mjSpec* spec = mj_makeSpec();
  mjsActuator* actuator = mjs_addActuator(spec, 0);

  double motorconst[2] = {0.05, 0.05};
  double resistance = 2.0;
  double lugre[5] = {100.0, 1.0, 0.5, 0.7, 10.0};

  const char* err =
      mjs_setToDCMotor(actuator, motorconst, resistance, nullptr, nullptr,
                       nullptr, nullptr, nullptr, nullptr, lugre, 0);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->dynprm[5], 100.0);  // stiffness
  EXPECT_EQ(actuator->dynprm[6], 1.0);    // damping
  EXPECT_EQ(actuator->biasprm[3], 0.5);   // coulomb
  EXPECT_EQ(actuator->biasprm[4], 0.7);   // static
  EXPECT_EQ(actuator->biasprm[5], 10.0);  // stribeck

  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, SetToOrientation) {
  mjSpec* spec = mj_makeSpec();
  mjsActuator* actuator = mjs_addActuator(spec, 0);

  // kv variant, default (expmap) chart
  double kv = 2.0;
  const char* err = mjs_setToOrientation(actuator, 5.0, &kv, nullptr, 0);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->gaintype, mjGAIN_SO3);
  EXPECT_EQ(actuator->biastype, mjBIAS_SO3);
  EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
  EXPECT_EQ(actuator->gainprm[0], 5.0);
  EXPECT_EQ(actuator->biasprm[1], -5.0);
  EXPECT_EQ(actuator->biasprm[2], -2.0);
  EXPECT_EQ(actuator->ctrlspec, 0);

  // dampratio variant, quat chart
  double dampratio = 1.0;
  err = mjs_setToOrientation(actuator, 5.0, nullptr, &dampratio, mjCHART_QUAT);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->biasprm[2], 1.0);
  EXPECT_EQ(actuator->ctrlspec, mjCHART_QUAT);

  // kv and dampratio are mutually exclusive
  err = mjs_setToOrientation(actuator, 5.0, &kv, &dampratio, 0);
  EXPECT_STREQ(err, "kv and dampratio cannot both be defined");

  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, SetToPID) {
  mjSpec* spec = mj_makeSpec();
  mjsActuator* actuator = mjs_addActuator(spec, 0);

  // stateless PID with kv, default input signature
  double kv = 3.0;
  const char* err = mjs_setToPID(actuator, 5.0, &kv, nullptr, nullptr, nullptr,
                                 nullptr, 0, 0);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->gaintype, mjGAIN_PID);
  EXPECT_EQ(actuator->biastype, mjBIAS_AFFINE);
  EXPECT_EQ(actuator->dyntype, mjDYN_NONE);
  EXPECT_EQ(actuator->biasprm[1], -5.0);
  EXPECT_EQ(actuator->biasprm[2], -3.0);
  EXPECT_EQ(actuator->gainprm[0], 0.0);

  // integral action with anti-windup, pos-only signature
  double ki = 0.5, imax = 2.0, dampratio = 1.0;
  err = mjs_setToPID(actuator, 5.0, nullptr, &dampratio, &ki, &imax, nullptr, 0,
                     mjINPUT_POS);
  EXPECT_STREQ(err, "");
  EXPECT_EQ(actuator->dyntype, mjDYN_PID);
  EXPECT_EQ(actuator->gainprm[0], 0.5);
  EXPECT_EQ(actuator->dynprm[0], 2.0);
  EXPECT_EQ(actuator->biasprm[2], 1.0);
  EXPECT_EQ(actuator->ctrlspec, mjINPUT_POS);

  // kv and dampratio are mutually exclusive
  err = mjs_setToPID(actuator, 5.0, &kv, &dampratio, nullptr, nullptr, nullptr,
                     0, 0);
  EXPECT_STREQ(err, "kv and dampratio cannot both be defined");

  mj_deleteSpec(spec);
}

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, SetNameDetectsRepeatedNames) {
  mjSpec* spec = mj_makeSpec();
  mjsBody* world = mjs_findBody(spec, "world");
  mjsGeom* geom1 = mjs_addGeom(world, 0);
  mjsGeom* geom2 = mjs_addGeom(world, 0);
  mjsSite* site = mjs_addSite(world, 0);

  // repeated name within a type is an error
  EXPECT_EQ(mjs_setName(geom1->element, "a"), 0);
  EXPECT_EQ(mjs_setName(geom2->element, "a"), -1);
  EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'a' in geom");

  // same name across types is fine
  EXPECT_EQ(mjs_setName(site->element, "a"), 0);

  // renaming an element frees its old name
  EXPECT_EQ(mjs_setName(geom1->element, "b"), 0);
  EXPECT_EQ(mjs_setName(geom2->element, "a"), 0);
  EXPECT_EQ(mjs_setName(geom1->element, "a"), -1);

  // a failed rename leaves the previous name in place
  EXPECT_STREQ(mjs_getName(geom1->element)->c_str(), "b");

  // renaming to the current name is fine
  EXPECT_EQ(mjs_setName(geom2->element, "a"), 0);

  // empty names may repeat
  EXPECT_EQ(mjs_setName(geom1->element, ""), 0);
  mjsGeom* geom3 = mjs_addGeom(world, 0);
  EXPECT_EQ(mjs_setName(geom3->element, ""), 0);

  // deleting an element frees its name
  EXPECT_EQ(mjs_delete(spec, geom2->element), 0);
  EXPECT_EQ(mjs_setName(geom3->element, "a"), 0);

  // frames are checked too
  mjsFrame* frame1 = mjs_addFrame(world, nullptr);
  mjsFrame* frame2 = mjs_addFrame(world, nullptr);
  EXPECT_EQ(mjs_setName(frame1->element, "f"), 0);
  EXPECT_EQ(mjs_setName(frame2->element, "f"), -1);
  EXPECT_THAT(mjs_getError(spec), HasSubstr("repeated name 'f'"));
  EXPECT_EQ(mjs_setName(frame2->element, "g"), 0);

  // names that arrived through attach are visible to the check
  mjSpec* child = mj_makeSpec();
  mjsBody* child_body = mjs_addBody(mjs_findBody(child, "world"), 0);
  EXPECT_EQ(mjs_setName(child_body->element, "attached"), 0);
  mjsFrame* frame = mjs_addFrame(world, nullptr);
  ASSERT_THAT(mjs_attach(frame->element, child_body->element, "", ""),
              NotNull());
  mjsBody* body = mjs_addBody(world, 0);
  EXPECT_EQ(mjs_setName(body->element, "attached"), -1);
  EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'attached' in body");
  EXPECT_EQ(mjs_setName(body->element, "own"), 0);

  // the model compiles once names are unique
  geom1->size[0] = geom3->size[0] = 1;
  mjModel* model = mj_compile(spec, 0);
  ASSERT_THAT(model, NotNull()) << mjs_getError(spec);
  EXPECT_EQ(mj_name2id(model, mjOBJ_GEOM, "a"), 1);
  EXPECT_EQ(mj_name2id(model, mjOBJ_BODY, "attached"), 1);

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

TEST_F(MujocoTest, SetNameDetectsRepeatedNamesFromXML) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <geom name="floor" type="plane" size="1 1 1"/>
      <body name="box">
        <geom name="boxgeom" size="0.1"/>
      </body>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> error;
  mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();

  // names loaded from XML are visible to the check
  mjsBody* world = mjs_findBody(spec, "world");
  mjsGeom* geom = mjs_addGeom(world, 0);
  geom->size[0] = 1;
  EXPECT_EQ(mjs_setName(geom->element, "floor"), -1);
  EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'floor' in geom");
  EXPECT_EQ(mjs_setName(geom->element, "boxgeom"), -1);
  EXPECT_EQ(mjs_setName(geom->element, "box"), 0);  // body name, other type

  // still checked after a compile
  mjModel* model = mj_compile(spec, 0);
  ASSERT_THAT(model, NotNull()) << mjs_getError(spec);
  mjsBody* body = mjs_addBody(world, 0);
  EXPECT_EQ(mjs_setName(body->element, "box"), -1);
  EXPECT_EQ(mjs_setName(body->element, "box2"), 0);

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

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

  // an edited spec has no signature until it is compiled
  EXPECT_EQ(spec->element->signature, 0);
  mjModel* model = mj_compile(spec, 0);
  ASSERT_THAT(model, NotNull()) << mjs_getError(spec);
  EXPECT_EQ(spec->element->signature, model->signature);

  // adding an element clears it, and deleting the element does not bring it
  // back
  mjsGeom* added = mjs_addGeom(body, 0);
  EXPECT_EQ(spec->element->signature, 0);
  EXPECT_EQ(mjs_delete(spec, added->element), 0);
  EXPECT_EQ(spec->element->signature, 0);

  // recompiling restores it
  mjModel* model2 = mj_compile(spec, 0);
  ASSERT_THAT(model2, NotNull()) << mjs_getError(spec);
  EXPECT_EQ(spec->element->signature, model2->signature);
  EXPECT_EQ(model2->signature, model->signature);

  // copying a compiled spec preserves the signature, so the copy still binds
  mjSpec* clean_copy = mj_copySpec(spec);
  EXPECT_EQ(clean_copy->element->signature, model2->signature);

  // attaching clears it
  mjSpec* child = mj_makeSpec();
  mjsBody* child_body = mjs_addBody(mjs_findBody(child, "world"), 0);
  mjsFrame* frame = mjs_addFrame(body, nullptr);
  ASSERT_THAT(mjs_attach(frame->element, child_body->element, "c_", ""),
              NotNull());
  EXPECT_EQ(spec->element->signature, 0);

  // copying an edited spec keeps it cleared
  mjSpec* dirty_copy = mj_copySpec(spec);
  EXPECT_EQ(dirty_copy->element->signature, 0);

  mjModel* model3 = mj_compile(spec, 0);
  ASSERT_THAT(model3, NotNull()) << mjs_getError(spec);
  EXPECT_EQ(spec->element->signature, model3->signature);
  EXPECT_NE(model3->signature, model2->signature);

  mj_deleteModel(model3);
  mj_deleteModel(model2);
  mj_deleteModel(model);
  mj_deleteSpec(dirty_copy);
  mj_deleteSpec(clean_copy);
  mj_deleteSpec(child);
  mj_deleteSpec(spec);
}

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);
  EXPECT_EQ(mjs_setName(mat1->element, "yellow"), 0);
  EXPECT_EQ(mjs_setName(mat2->element, "yellow"), -1);
  EXPECT_STREQ(mjs_getError(spec), "Error: repeated name 'yellow' in material");

  EXPECT_EQ(mj_recompile(spec, 0, model, data), -1);
  EXPECT_THAT(mjs_getError(spec), HasSubstr("empty name 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);
}

// the bounding volume hierarchy of a body is rebuilt by every compilation
TEST_F(MujocoTest, RecompileEditGeoms) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <geom name="a" size=".1"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  static constexpr char xml_two_geoms[] = R"(
  <mujoco>
    <worldbody>
      <body name="body">
        <geom name="a" size=".1"/>
        <geom name="b" size=".1" pos="1 0 0"/>
      </body>
    </worldbody>
  </mujoco>
  )";

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

  // add a geom: the root is no longer a leaf
  mjsGeom* geom = mjs_addGeom(mjs_findBody(spec, "body"), nullptr);
  mjs_setName(geom->element, "b");
  geom->size[0] = .1;
  geom->pos[0] = 1;
  mjModel* m2 = mj_compile(spec, nullptr);
  ASSERT_THAT(m2, NotNull()) << mjs_getError(spec);
  MjModelPtr expected =
      LoadModelFromString(xml_two_geoms, er.data(), er.size());
  ASSERT_THAT(expected.get(), NotNull()) << er.data();
  ASSERT_EQ(m2->nbvh, expected->nbvh);
  for (int i = 0; i < m2->nbvh; i++) {
    EXPECT_EQ(m2->bvh_nodeid[i], expected->bvh_nodeid[i]) << "node " << i;
  }

  // delete both geoms: no hierarchy is left
  EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_GEOM, "a")), 0);
  EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_GEOM, "b")), 0);
  mjModel* m3 = mj_compile(spec, nullptr);
  ASSERT_THAT(m3, NotNull()) << mjs_getError(spec);
  EXPECT_EQ(m3->nbvh, 0);
  EXPECT_EQ(m3->body_bvhnum[1], 0);

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

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);
}

// editing a frame's pose after a compile takes effect in the next compile
TEST_F(MujocoTest, RecompileEditFrame) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <frame name="outer" pos="0 0 1">
        <frame name="inner" pos="1 0 0">
          <geom name="geom" size=".1"/>
          <body name="body">
            <freejoint/>
            <geom size=".1"/>
          </body>
        </frame>
      </frame>
    </worldbody>
  </mujoco>
  )";

  std::array<char, 1000> er;
  mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
  ASSERT_THAT(spec, NotNull()) << er.data();
  mjModel* m1 = mj_compile(spec, nullptr);
  ASSERT_THAT(m1, NotNull());
  int geom = mj_name2id(m1, mjOBJ_GEOM, "geom");
  int body = mj_name2id(m1, mjOBJ_BODY, "body");
  const mjtNum tol = MjTol(1e-12, 1e-6);
  const mjtNum pos1[3] = {1, 0, 1};
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(m1->geom_pos[3 * geom + i], pos1[i], tol);
    EXPECT_NEAR(m1->body_pos[3 * body + i], pos1[i], tol);
  }

  // move the outer frame; move the inner one and rotate it 90 degrees about z
  mjsFrame* outer = mjs_findFrame(spec, "outer");
  mjsFrame* inner = mjs_findFrame(spec, "inner");
  outer->pos[2] = 5;
  inner->pos[0] = 0;
  inner->pos[1] = 2;
  inner->quat[0] = inner->quat[3] = mju_sqrt(0.5);

  mjModel* m2 = mj_compile(spec, nullptr);
  ASSERT_THAT(m2, NotNull());
  const mjtNum pos2[3] = {0, 2, 5};
  const mjtNum quat2[4] = {mju_sqrt(0.5), 0, 0, mju_sqrt(0.5)};
  for (int i = 0; i < 3; i++) {
    EXPECT_NEAR(m2->geom_pos[3 * geom + i], pos2[i], tol);
    EXPECT_NEAR(m2->body_pos[3 * body + i], pos2[i], tol);
    EXPECT_NEAR(m2->qpos0[i], pos2[i], tol);
  }
  for (int i = 0; i < 4; i++) {
    EXPECT_NEAR(m2->body_quat[4 * body + i], quat2[i], tol);
  }

  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
  MjModelPtr m =
      LoadModelFromString(xml, error.data(), error.size(), vfs.get());
  EXPECT_EQ(m->mesh_vert[0], -0.5);

  // 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_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
  MjModelPtr 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];

  // 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_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
  MjModelPtr 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_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
  MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected.get(), field), tol)
      << "Expected and attached models are different!\n"
      << "Different field: " << field << '\n';
  ;

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

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());

  mjsTendon* tendon = mjs_asTendon(
      mjs_findElement(parent, mjOBJ_TENDON, "tendon_with_sidesite_child"));
  ASSERT_THAT(tendon, NotNull());
  ASSERT_EQ(mjs_getWrapNum(tendon), 3);

  EXPECT_EQ(mjs_getWrapTarget(mjs_getWrap(tendon, 0)),
            mjs_findElement(parent, mjOBJ_SITE, "site_A_child"));
  EXPECT_EQ(mjs_getWrapTarget(mjs_getWrap(tendon, 1)),
            mjs_findElement(parent, mjOBJ_GEOM, "wrap_geom_child"));
  EXPECT_EQ(mjs_getWrapTarget(mjs_getWrap(tendon, 2)),
            mjs_findElement(parent, mjOBJ_SITE, "site_B_child"));
  EXPECT_EQ(mjs_getWrapSideSite(mjs_getWrap(tendon, 1)),
            mjs_asSite(mjs_findElement(parent, mjOBJ_SITE, "side_site_child")));

  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
  MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected.get(), 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);
}

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
  MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected.get(), 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);
}

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
  MjModelPtr m_expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_detached, m_expected.get(), field), tol)
      << "Expected and attached models are different!\n"
      << "Different field: " << field << '\n';

  // destroy everything
  mj_deleteSpec(child);
  mj_deleteModel(m_detached);
  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());
  MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected.get(), field), tol)
      << "Expected and attached models are different!\n"
      << "Different field: " << field << '\n';

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

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());
  MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected.get(), field), tol)
      << "Expected and attached models are different!\n"
      << "Different field: " << field << '\n';

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

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());
  MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(model2, expected.get(), 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);
}

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());
  MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected.get(), 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);
}

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());
  MjModelPtr expected = LoadModelFromString(xml_result, er.data(), er.size());
  EXPECT_THAT(expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected.get(), 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);
}

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());
  MjModelPtr m_expected =
      LoadModelFromString(xml_expected, er.data(), er.size());
  EXPECT_THAT(m_expected.get(), NotNull());

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

  // set ctrl
  data->ctrl[0] = 1;
  data->ctrl[1] = 2;
  d_expected.get()->ctrl[0] = 2;

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

  // step models
  mj_step(model, data);
  mj_step(m_expected.get(), d_expected.get());
  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.get()->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.get()->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.get()->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.get()->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.get()->mocap_pos[3 * i + j])
          << i;
    }
    for (int j = 0; j < 4; ++j) {
      EXPECT_EQ(data->mocap_quat[4 * i + j],
                d_expected.get()->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_deleteSpec(spec);
  mj_deleteModel(model);
}

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, RecompileControlBlocks) {
  std::array<char, 1000> er;

  // 1. Deleting first actuator preserves retained actuator's control
  static constexpr char xml_delete[] = R"(
  <mujoco>
    <worldbody>
      <body>
        <joint name="j1" type="slide"/>
        <geom size="0.1"/>
      </body>
      <body>
        <joint name="j2" type="slide"/>
        <geom size="0.1"/>
      </body>
    </worldbody>
    <actuator>
      <motor name="a1" joint="j1"/>
      <motor name="a2" joint="j2"/>
    </actuator>
  </mujoco>)";

  mjSpec* s1 = mj_parseXMLString(xml_delete, 0, er.data(), er.size());
  ASSERT_THAT(s1, NotNull()) << er.data();
  mjModel* m1 = mj_compile(s1, 0);
  ASSERT_THAT(m1, NotNull());
  mjData* d1 = mj_makeData(m1);
  d1->ctrl[0] = 11.0;
  d1->ctrl[1] = 22.0;

  mjsElement* a1 = mjs_findElement(s1, mjOBJ_ACTUATOR, "a1");
  ASSERT_THAT(a1, NotNull());
  EXPECT_EQ(mjs_delete(s1, a1), 0);
  EXPECT_EQ(mj_recompile(s1, 0, m1, d1), 0);
  EXPECT_EQ(m1->nu, 1);
  EXPECT_MJTNUM_EQ(d1->ctrl[0], 22.0);
  mj_deleteData(d1);
  mj_deleteModel(m1);
  mj_deleteSpec(s1);

  // 2. Multi-input PID actuator preserves all control slots
  static constexpr char xml_pid[] = R"(
  <mujoco>
    <worldbody>
      <body>
        <joint name="j" type="slide"/>
        <geom size="0.1"/>
      </body>
    </worldbody>
    <actuator>
      <pid name="pid" joint="j" kp="10" kv="1" input="pos vel ff"/>
    </actuator>
  </mujoco>)";

  mjSpec* s2 = mj_parseXMLString(xml_pid, 0, er.data(), er.size());
  ASSERT_THAT(s2, NotNull()) << er.data();
  mjModel* m2 = mj_compile(s2, 0);
  ASSERT_THAT(m2, NotNull());
  ASSERT_EQ(m2->nu, 3);
  mjData* d2 = mj_makeData(m2);
  d2->ctrl[0] = 1.0;
  d2->ctrl[1] = 2.0;
  d2->ctrl[2] = 5.0;

  EXPECT_EQ(mj_recompile(s2, 0, m2, d2), 0);
  ASSERT_EQ(m2->nu, 3);
  EXPECT_MJTNUM_EQ(d2->ctrl[0], 1.0);
  EXPECT_MJTNUM_EQ(d2->ctrl[1], 2.0);
  EXPECT_MJTNUM_EQ(d2->ctrl[2], 5.0);
  mj_deleteData(d2);
  mj_deleteModel(m2);
  mj_deleteSpec(s2);

  // 3. Zero-input actuator followed by scalar actuator (nactuator=2, nu=1)
  static constexpr char xml_zero[] = R"(
  <mujoco>
    <worldbody>
      <body>
        <joint name="j1" type="slide"/>
        <geom size="0.1"/>
      </body>
      <body>
        <joint name="j2" type="slide"/>
        <geom size="0.1"/>
      </body>
    </worldbody>
    <actuator>
      <dcmotor name="dc0" joint="j1" input="none" motorconst="1" resistance="1"/>
      <motor name="m1" joint="j2"/>
    </actuator>
  </mujoco>)";

  mjSpec* s3 = mj_parseXMLString(xml_zero, 0, er.data(), er.size());
  ASSERT_THAT(s3, NotNull()) << er.data();
  mjModel* m3 = mj_compile(s3, 0);
  ASSERT_THAT(m3, NotNull());
  ASSERT_EQ(m3->nactuator, 2);
  ASSERT_EQ(m3->nu, 1);
  mjData* d3 = mj_makeData(m3);
  d3->ctrl[0] = 33.0;

  EXPECT_EQ(mj_recompile(s3, 0, m3, d3), 0);
  ASSERT_EQ(m3->nactuator, 2);
  ASSERT_EQ(m3->nu, 1);
  EXPECT_MJTNUM_EQ(d3->ctrl[0], 33.0);
  mj_deleteData(d3);
  mj_deleteModel(m3);
  mj_deleteSpec(s3);

  // 4. Expanding actuator actdim from 1 to 3 does not read past saved act
  static constexpr char xml_actdim[] = R"(
  <mujoco>
    <worldbody>
      <body>
        <joint name="j" type="hinge"/>
        <geom size="0.1"/>
      </body>
    </worldbody>
    <actuator>
      <general name="a" joint="j" dyntype="integrator"/>
    </actuator>
  </mujoco>)";

  mjSpec* s4 = mj_parseXMLString(xml_actdim, 0, er.data(), er.size());
  ASSERT_THAT(s4, NotNull()) << er.data();
  mjModel* m4 = mj_compile(s4, 0);
  ASSERT_THAT(m4, NotNull());
  ASSERT_EQ(m4->na, 1);
  mjData* d4 = mj_makeData(m4);
  d4->act[0] = 42.0;

  mjsActuator* act4 = mjs_asActuator(mjs_findElement(s4, mjOBJ_ACTUATOR, "a"));
  ASSERT_THAT(act4, NotNull());
  act4->dyntype = mjDYN_USER;
  act4->actdim = 3;

  EXPECT_EQ(mj_recompile(s4, 0, m4, d4), 0);
  ASSERT_EQ(m4->na, 3);
  EXPECT_MJTNUM_EQ(d4->act[0], 42.0);
  EXPECT_MJTNUM_EQ(d4->act[1], 0.0);
  EXPECT_MJTNUM_EQ(d4->act[2], 0.0);
  mj_deleteData(d4);
  mj_deleteModel(m4);
  mj_deleteSpec(s4);
}

TEST_F(MujocoTest, RecompileIntegrationState) {
  std::array<char, 1000> er;
  static constexpr char xml[] = R"(
  <mujoco>
    <size nuserdata="2"/>
    <worldbody>
      <body name="b1">
        <joint name="j1" type="slide" axis="1 0 0"/>
        <geom size="0.1" mass="1"/>
      </body>
      <body name="b2">
        <joint name="j2" type="slide" axis="1 0 0"/>
        <geom size="0.1" mass="1"/>
      </body>
    </worldbody>
    <equality>
      <joint name="eq1" joint1="j1" active="false"/>
      <joint name="eq2" joint1="j2" active="false"/>
    </equality>
    <actuator>
      <motor name="a1" joint="j1" nsample="2" delay="0.01"/>
    </actuator>
    <sensor>
      <jointpos name="s1" joint="j2" nsample="2" delay="0.01"/>
    </sensor>
  </mujoco>)";

  mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size());
  ASSERT_THAT(spec, NotNull()) << er.data();
  mjModel* m = mj_compile(spec, 0);
  ASSERT_THAT(m, NotNull());
  mjData* d = mj_makeData(m);
  ASSERT_THAT(d, NotNull());

  d->time = 3.25;
  d->qpos[0] = 0.4;
  d->qpos[1] = -0.2;
  d->qvel[0] = 0.3;
  d->qvel[1] = -0.1;
  d->qfrc_applied[0] = 7.0;
  d->qfrc_applied[1] = 9.0;
  d->xfrc_applied[6 * 1 + 0] = 8.0;
  d->xfrc_applied[6 * 2 + 2] = 12.0;
  d->eq_active[0] = 1;
  d->eq_active[1] = 1;
  d->userdata[0] = 123.0;
  d->userdata[1] = 321.0;
  d->qacc_warmstart[0] = 456.0;
  d->qacc_warmstart[1] = 654.0;
  d->ctrl[0] = 4.5;
  for (int i = 0; i < m->nhistory; i++) {
    d->history[i] = 10.0 + i;
  }

  int nstate = mj_stateSize(m, mjSTATE_INTEGRATION);
  std::vector<mjtNum> state_before(nstate);
  mj_getState(m, d, state_before.data(), mjSTATE_INTEGRATION);

  // no-op recompile preserves the entire mjSTATE_INTEGRATION
  EXPECT_EQ(mj_recompile(spec, 0, m, d), 0);
  std::vector<mjtNum> state_after(nstate);
  mj_getState(m, d, state_after.data(), mjSTATE_INTEGRATION);
  EXPECT_EQ(state_before, state_after);

  // delete b1 (along with j1, eq1, a1) and recompile: b2, j2, eq2, s1 state
  // preserved
  mjsBody* b1 = mjs_findBody(spec, "b1");
  ASSERT_THAT(b1, NotNull());
  EXPECT_EQ(mjs_delete(spec, b1->element), 0);
  EXPECT_EQ(mj_recompile(spec, 0, m, d), 0);
  EXPECT_EQ(m->nq, 1);
  EXPECT_EQ(m->nv, 1);
  EXPECT_EQ(m->nbody, 2);
  EXPECT_EQ(m->neq, 1);
  EXPECT_MJTNUM_EQ(d->qpos[0], -0.2);
  EXPECT_MJTNUM_EQ(d->qvel[0], -0.1);
  EXPECT_MJTNUM_EQ(d->qfrc_applied[0], 9.0);
  EXPECT_MJTNUM_EQ(d->qacc_warmstart[0], 654.0);
  EXPECT_MJTNUM_EQ(d->xfrc_applied[6 * 1 + 2], 12.0);
  EXPECT_EQ(d->eq_active[0], 1);
  EXPECT_MJTNUM_EQ(d->userdata[0], 123.0);
  EXPECT_MJTNUM_EQ(d->userdata[1], 321.0);
  ASSERT_EQ(m->nhistory, 6);
  for (int i = 0; i < 6; i++) {
    EXPECT_MJTNUM_EQ(d->history[i], 16.0 + i);
  }

  // add a new body/joint/actuator/equality and mutate j2 from slide to ball
  mjsBody* b3 = mjs_addBody(mjs_findBody(spec, "world"), 0);
  mjsGeom* g3 = mjs_addGeom(b3, 0);
  g3->size[0] = 0.1;
  g3->mass = 1.0;
  mjsJoint* j3 = mjs_addJoint(b3, 0);
  mjs_setName(j3->element, "j3");
  j3->type = mjJNT_SLIDE;
  j3->ref = 0.75;
  mjsActuator* a3 = mjs_addActuator(spec, 0);
  a3->trntype = mjTRN_JOINT;
  mjs_setString(a3->target, "j3");
  mjsEquality* eq3 = mjs_addEquality(spec, 0);
  eq3->type = mjEQ_JOINT;
  eq3->active = 1;
  mjs_setString(eq3->name1, "j3");

  // delete eq2 and s1 before mutating j2 to ball joint
  EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_EQUALITY, "eq2")), 0);
  EXPECT_EQ(mjs_delete(spec, mjs_findElement(spec, mjOBJ_SENSOR, "s1")), 0);
  mjsJoint* j2 = mjs_asJoint(mjs_findElement(spec, mjOBJ_JOINT, "j2"));
  ASSERT_THAT(j2, NotNull());
  j2->type = mjJNT_BALL;

  EXPECT_EQ(mj_recompile(spec, 0, m, d), 0);
  ASSERT_EQ(m->nq, 5);
  ASSERT_EQ(m->nv, 4);
  // j2 changed type from slide to ball: initialized to unit quaternion from
  // qpos0 and zero vel
  EXPECT_MJTNUM_EQ(d->qpos[0], 1.0);
  EXPECT_MJTNUM_EQ(d->qpos[1], 0.0);
  EXPECT_MJTNUM_EQ(d->qpos[2], 0.0);
  EXPECT_MJTNUM_EQ(d->qpos[3], 0.0);
  EXPECT_MJTNUM_EQ(d->qvel[0], 0.0);
  EXPECT_MJTNUM_EQ(d->qfrc_applied[0], 0.0);
  EXPECT_MJTNUM_EQ(d->qacc_warmstart[0], 0.0);
  // newly added j3, a3, eq3 receive defaults while b2 retains xfrc_applied
  EXPECT_MJTNUM_EQ(d->qpos[4], 0.75);
  EXPECT_MJTNUM_EQ(d->qvel[3], 0.0);
  EXPECT_MJTNUM_EQ(d->ctrl[0], 0.0);
  EXPECT_EQ(d->eq_active[0], 1);
  EXPECT_MJTNUM_EQ(d->xfrc_applied[6 * 1 + 2], 12.0);
  EXPECT_MJTNUM_EQ(d->xfrc_applied[6 * 2 + 2], 0.0);

  mj_deleteData(d);
  mj_deleteModel(m);
  mj_deleteSpec(spec);
}

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());

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

  mj_deleteSpec(spec);
  mj_deleteModel(model);
}

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;
  MjModelPtr m = LoadModelFromString(xml, error.data(), error.size());
  EXPECT_THAT(m.get(), 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.get(), mjOBJ_KEY, 0), "keyframe0");
  EXPECT_STREQ(mj_id2name(m.get(), mjOBJ_KEY, 1), "keyframe");
}

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
  MockWarningHandler warning_handler;
  if (!compile) {
    warning_handler.ExpectWarnings("model has pending keyframes");
  }

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

  if (compile) {
    EXPECT_EQ(mjs_numWarnings(parent), 0);
  } else {
    EXPECT_GE(mjs_numWarnings(parent), 1);
    EXPECT_THAT(mjs_getWarning(parent, 0),
                HasSubstr("model has pending keyframes"));
  }
  // compare models
  mjtNum tol = 0;
  std::string field = "";
  mjModel* m_attached = mj_compile(parent, 0);
  EXPECT_THAT(m_attached, NotNull());
  MjModelPtr m_expected = LoadModelFromString(
      compile ? expected_xml : expected_xml_uncompiled, er.data(), er.size());
  EXPECT_THAT(m_expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(m_attached, m_expected.get(), 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_attached);
  mj_deleteModel(m_child);
}

TEST_F(MujocoTest, TestAttachNestedKeyframe) {
  mock_warning_handler.ExpectWarnings();
  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 = "";
  MjModelPtr expected = LoadModelFromString(xml_expected, er.data(), er.size());
  EXPECT_THAT(expected.get(), NotNull()) << er.data();
  EXPECT_LE(CompareModel(model, expected.get(), field), tol)
      << "Expected and attached models are different!\n"
      << "Different field: " << field << '\n';

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

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, CopyInertialInFrame) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body>
        <frame name="frame" pos="1 0 0">
          <inertial pos="0 1 0" mass="1" diaginertia="1 1 1"/>
        </frame>
      </body>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> error;
  mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();

  // move the frame in a copy, the copied inertial follows its own frame
  mjSpec* copy = mj_copySpec(spec);
  ASSERT_THAT(copy, NotNull());
  mjs_findFrame(copy, "frame")->pos[0] = 2;
  mjModel* model = mj_compile(copy, 0);
  ASSERT_THAT(model, NotNull());
  EXPECT_EQ(model->body_ipos[3], 2);
  EXPECT_EQ(model->body_ipos[4], 1);
  EXPECT_EQ(model->body_ipos[5], 0);

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

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);
}

TEST_F(MujocoTest, CompilerTimers) {
  static constexpr char xml[] = R"(
  <mujoco>
    <asset>
      <texture name="grid" type="2d" builtin="checker" width="300" height="300" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
      <material name="grid" texture="grid"/>
    </asset>
    <worldbody>
      <geom type="plane" size="1 1 1" material="grid"/>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> error;
  mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();

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

  EXPECT_GT(mjs_getTimer(spec)[mjCTIMER_TOTAL], 0);
  EXPECT_GT(mjs_getTimer(spec)[mjCTIMER_ASSETS], 0);
  EXPECT_GT(mjs_getTimer(spec)[mjCTIMER_TEXTURE], 0);

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

// -------------------- test compile warning infrastructure --------------------

TEST_F(MujocoTest, CompileWarningCount) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent">
        <geom size="1"/>
        <flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
                  dim="2" radius="0.01">
        </flexcomp>
      </body>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> error;
  mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();

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

  // flex with no passive forces should produce a warning
  EXPECT_GT(mjs_numWarnings(spec), 0);
  EXPECT_THAT(mjs_getWarning(spec, 0), HasSubstr("not rigid"));

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

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

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

  // no warnings expected for simple model
  EXPECT_EQ(mjs_numWarnings(spec), 0);
  EXPECT_THAT(mjs_getWarning(spec, 0), IsNull());
  EXPECT_THAT(mjs_getWarning(spec, -1), IsNull());

  // nullptr spec should not crash
  EXPECT_EQ(mjs_numWarnings(nullptr), 0);
  EXPECT_THAT(mjs_getWarning(nullptr, 0), IsNull());

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, RecompileClearsCompileWarnings) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent">
        <geom size="1"/>
        <flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
                  dim="2" radius="0.01">
        </flexcomp>
      </body>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> error;
  mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();

  mjModel* model = mj_compile(spec, 0);
  ASSERT_THAT(model, NotNull());
  int first_count = mjs_numWarnings(spec);
  EXPECT_GT(first_count, 0);

  // recompile — warnings should be regenerated, not accumulated
  mj_deleteModel(model);
  model = mj_compile(spec, 0);
  ASSERT_THAT(model, NotNull());
  EXPECT_EQ(mjs_numWarnings(spec), first_count);

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, LoadXMLWarningInErrorBuffer) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent">
        <geom size="1"/>
        <flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
                  dim="2" radius="0.01">
        </flexcomp>
      </body>
    </worldbody>
  </mujoco>
  )";

  // write xml to VFS
  mjVFS vfs;
  mj_defaultVFS(&vfs);
  mj_addBufferVFS(&vfs, "model.xml", xml, sizeof(xml));

  std::array<char, 1024> error;
  error[0] = '\0';
  mjModel* model = mj_loadXML("model.xml", &vfs, error.data(), error.size());
  ASSERT_THAT(model, NotNull());

  // warning should be in the error buffer
  EXPECT_THAT(error.data(), HasSubstr("not rigid"));

  mj_deleteModel(model);
  mj_deleteVFS(&vfs);
}

TEST_F(MujocoTest, CompileWarningChainedToHandler) {
  static constexpr char xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="parent">
        <geom size="1"/>
        <flexcomp name="grid" type="grid" count="3 3 1" spacing="0.1 0.1 0.1"
                  dim="2" radius="0.01">
        </flexcomp>
      </body>
    </worldbody>
  </mujoco>
  )";
  std::array<char, 1024> error;
  mjSpec* spec = mj_parseXMLString(xml, 0, error.data(), error.size());
  ASSERT_THAT(spec, NotNull()) << error.data();

  // install a custom log handler that captures warnings
  std::vector<std::string> captured_warnings;
  static thread_local std::vector<std::string>* capture_ptr = nullptr;
  capture_ptr = &captured_warnings;

  // install custom log handler (replaces global, so mock is bypassed)

  mjfLogHandler prev = mju_setLogHandler([](const mjLogMessage* msg) {
    if (msg->level == mjLOG_WARNING && capture_ptr) {
      capture_ptr->push_back(msg->subject);
    }
  });

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

  // restore log handler
  mju_setLogHandler(prev);
  capture_ptr = nullptr;

  // chaining should have forwarded warnings to our handler
  EXPECT_THAT(captured_warnings, testing::Contains(HasSubstr("not rigid")));

  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, WarningAccumulationAndRetrieval) {
  mock_warning_handler.ExpectWarnings();
  static constexpr char xml_parent[] = R"(
  <mujoco>
    <worldbody>
      <frame name="parent"/>
    </worldbody>
  </mujoco>
  )";

  static constexpr char xml_child[] = R"(
  <mujoco>
    <worldbody>
      <body name="child">
        <frame name="child_frame"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  static constexpr char xml_gchild[] = R"(
  <mujoco>
    <worldbody>
      <body name="gchild"/>
    </worldbody>
    <keyframe>
      <key name="k1"/>
    </keyframe>
  </mujoco>
  )";

  std::array<char, 1024> 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();
  mjSpec* gchild = mj_parseXMLString(xml_gchild, 0, er.data(), er.size());
  ASSERT_THAT(gchild, NotNull()) << er.data();

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

  mjs_attach(mjs_findFrame(child, "child_frame")->element,
             mjs_findBody(gchild, "gchild")->element, "gchild-", "");

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

  EXPECT_EQ(mjs_numWarnings(parent), 1);
  EXPECT_THAT(mjs_getWarning(parent, 0),
              HasSubstr("Child model has pending keyframes"));
  EXPECT_THAT(mjs_getWarning(parent, 1), IsNull());

  mj_deleteSpec(parent);
  mj_deleteSpec(child);
  mj_deleteSpec(gchild);
}

TEST_F(MujocoTest, CompilationWarningsClearedOnRecompile) {
  mock_warning_handler.ExpectWarnings();
  // flex with no edge stiffness or equality triggers passive forces warning
  static constexpr char xml[] = R"(
  <mujoco>
  <worldbody>
    <flexcomp name="test" type="grid" count="4 4 1" spacing=".2 .2 .2"
              dim="2" radius=".1"/>
  </worldbody>
  </mujoco>
  )";

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

  // first compile: should generate flex warning
  mjModel* m = mj_compile(spec, nullptr);
  ASSERT_THAT(m, NotNull());
  int n1 = mjs_numWarnings(spec);
  EXPECT_EQ(n1, 1);
  EXPECT_THAT(mjs_getWarning(spec, 0),
              HasSubstr("no equality constraints or passive forces"));

  // recompile: warnings should be cleared and regenerated
  mj_deleteModel(m);
  m = mj_compile(spec, nullptr);
  ASSERT_THAT(m, NotNull());
  EXPECT_EQ(mjs_numWarnings(spec), n1);

  mj_deleteModel(m);
  mj_deleteSpec(spec);
}

TEST_F(MujocoTest, MjEncodeNativeFormats) {
  // simple test spec
  mjSpec* spec = mj_makeSpec();
  mjsBody* world = mjs_findBody(spec, "world");
  mjsBody* body = mjs_addBody(world, nullptr);
  mjsGeom* geom = mjs_addGeom(body, nullptr);
  geom->size[0] = 1.0;
  geom->size[1] = 1.0;
  geom->size[2] = 1.0;

  mjModel* model = mj_compile(spec, nullptr);
  ASSERT_THAT(model, NotNull());

  std::filesystem::path tmp_dir = std::filesystem::temp_directory_path();
  std::string xml_path = (tmp_dir / "test_encode.xml").string();
  std::string mjb_path = (tmp_dir / "test_encode.mjb").string();
  std::string txt_path = (tmp_dir / "test_encode.txt").string();

  char error[1000];

  // XML Encoding with spec
  EXPECT_GT(mj_encode(spec, model, xml_path.c_str(), nullptr, nullptr, error,
                      sizeof(error)),
            0);
  EXPECT_TRUE(std::filesystem::exists(xml_path));
  EXPECT_GT(std::filesystem::file_size(xml_path), 0);

  // XML Encoding without spec (should fail because no XML loaded in global
  // spec)
  std::filesystem::remove(xml_path);
  // free global spec
  mj_freeLastXML();
  EXPECT_EQ(mj_encode(nullptr, model, xml_path.c_str(), nullptr, nullptr, error,
                      sizeof(error)),
            -1);
  EXPECT_THAT(error, HasSubstr("No XML model loaded"));
  EXPECT_TRUE(std::filesystem::exists(xml_path));
  EXPECT_EQ(std::filesystem::file_size(xml_path), 0);
  std::filesystem::remove(xml_path);

  // MJB Encoding (spec can be null)
  EXPECT_GT(mj_encode(nullptr, model, mjb_path.c_str(), nullptr, nullptr, error,
                      sizeof(error)),
            0);
  EXPECT_TRUE(std::filesystem::exists(mjb_path));
  EXPECT_GT(std::filesystem::file_size(mjb_path), 0);

  mjModel* model2 = mj_loadModel(mjb_path.c_str(), nullptr);
  EXPECT_THAT(model2, NotNull());
  mj_deleteModel(model2);

  // TXT Encoding (spec can be null)
  EXPECT_GT(mj_encode(nullptr, model, txt_path.c_str(), nullptr, nullptr, error,
                      sizeof(error)),
            0);
  EXPECT_TRUE(std::filesystem::exists(txt_path));
  EXPECT_GT(std::filesystem::file_size(txt_path), 0);

  // Clean up
  std::filesystem::remove(mjb_path);
  std::filesystem::remove(txt_path);
  mj_deleteModel(model);
  mj_deleteSpec(spec);
}

// Tests that joint ordering is preserved after attach operations
TEST_F(MujocoTest, AttachPreservesJointOrder) {
  static constexpr char child_xml[] = R"(
  <mujoco>
    <worldbody>
      <body name="slider_body">
        <joint name="slide_joint" type="slide"/>
        <geom type="box" size="0.1 0.1 0.1"/>
      </body>
      <body name="free_body" pos="1 0 0">
        <freejoint name="free_joint"/>
        <geom type="sphere" size="0.1"/>
      </body>
    </worldbody>
  </mujoco>
  )";

  mjSpec* parent = mj_makeSpec();
  mjsBody* parent_world = mjs_findBody(parent, "world");
  mjsFrame* attach_frame = mjs_addFrame(parent_world, nullptr);
  mjs_setName(attach_frame->element, "attach_frame");

  std::array<char, 1000> error;
  mjSpec* child =
      mj_parseXMLString(child_xml, nullptr, error.data(), error.size());
  ASSERT_THAT(child, NotNull()) << error.data();

  mjsElement* attached =
      mjs_attach(attach_frame->element, child->element, "child_", "");
  ASSERT_THAT(attached, NotNull());

  mjModel* model = mj_compile(parent, nullptr);
  ASSERT_THAT(model, NotNull());

  // Verify joint order: slide joint should come before free joint
  int slide_id = mj_name2id(model, mjOBJ_JOINT, "child_slide_joint");
  int free_id = mj_name2id(model, mjOBJ_JOINT, "child_free_joint");
  ASSERT_GE(slide_id, 0);
  ASSERT_GE(free_id, 0);
  EXPECT_LT(slide_id, free_id)
      << "Slide joint should have a lower ID than free joint";

  // Export and re-import to test round-trip preservation
  std::string exported_xml = SaveAndReadXml(parent);

  mjSpec* reimported = mj_parseXMLString(exported_xml.c_str(), nullptr,
                                         error.data(), error.size());
  ASSERT_THAT(reimported, NotNull()) << error.data();

  mjModel* reimported_model = mj_compile(reimported, nullptr);
  ASSERT_THAT(reimported_model, NotNull());

  // Verify joint order is preserved after round-trip
  int reimported_slide_id =
      mj_name2id(reimported_model, mjOBJ_JOINT, "child_slide_joint");
  int reimported_free_id =
      mj_name2id(reimported_model, mjOBJ_JOINT, "child_free_joint");
  ASSERT_GE(reimported_slide_id, 0);
  ASSERT_GE(reimported_free_id, 0);
  EXPECT_EQ(reimported_slide_id, slide_id)
      << "Joint IDs should be the same after XML round-trip";
  EXPECT_EQ(reimported_free_id, free_id)
      << "Joint IDs should be the same after XML round-trip";

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

TEST_F(MujocoTest, DeleteReclaimsMemoryImmediately) {
  mjSpec* spec = mj_makeSpec();
  mjsBody* world = mjs_findBody(spec, "world");

  int cleanup_count = 0;
  auto cleanup = +[](const void* data) {
    *static_cast<int*>(const_cast<void*>(data)) += 1;
  };

  mjsBody* body = mjs_addBody(world, nullptr);
  mjsGeom* geom = mjs_addGeom(body, nullptr);
  mjs_setUserValueWithCleanup(body->element, "tracker", &cleanup_count,
                              cleanup);
  mjs_setUserValueWithCleanup(geom->element, "tracker", &cleanup_count,
                              cleanup);

  EXPECT_EQ(cleanup_count, 0);
  EXPECT_EQ(mjs_delete(spec, body->element), 0);
  EXPECT_EQ(cleanup_count, 2);

  mj_deleteSpec(spec);
  EXPECT_EQ(cleanup_count, 2);
}
}  // namespace
}  // namespace mujoco