#ifndef MUJOCO_MJWRAPPER_H_
#define MUJOCO_MJWRAPPER_H_

#include <functional>
#include <memory>
#include <mujoco/mujoco.h>

namespace tinyxml2 {
class XMLDocument;
class XMLElement;
} // namespace tinyxml2

struct mjVec_t {
  mjtNum x, y, z;
  mjVec_t() : x(0), y(0), z(0) {}
  mjVec_t(mjtNum x, mjtNum y, mjtNum z) : x(x), y(y), z(z) {}
  std::string ToString() const {
    return std::to_string(x) + " " + std::to_string(y) + " " +
           std::to_string(z);
  }
};

struct mjQuat_t {
  double x, y, z, w;

  mjQuat_t() : x(0), y(0), z(0), w(1) {}
  mjQuat_t(mjtNum x, mjtNum y, mjtNum z, mjtNum w) : x(x), y(y), z(z), w(w) {}
  std::string ToString() const {
    return std::to_string(w) + " " + std::to_string(x) + " " +
           std::to_string(y) + " " + std::to_string(z);
  }
};

class FMujocoWrapper {

  std::unique_ptr<mjSpec, std::function<void(mjSpec *)>> Spec;

  std::unique_ptr<tinyxml2::XMLDocument> XmlDoc;
  std::unique_ptr<mjVFS> Vfs;

  tinyxml2::XMLElement *RootElement;
  tinyxml2::XMLElement *AssetsElement;

public:
  MJAPI FMujocoWrapper();
  MJAPI ~FMujocoWrapper();

  MJAPI void ParseXml(const std::string &xml_string);

  MJAPI void AppendXml(const std::string &xml_string,
                       const std::string &base_path,
                       const std::string &file_name,
                       const std::string &model_name,
                       const std::string &prefix,
                       bool is_static, const mjVec_t &pos,
                       const mjQuat_t &quat);

  MJAPI void RemoveBody(const std::string &body_name);

  MJAPI void AppendBinary(const std::vector<uint8_t>& buffer, const std::string &prefix);

  MJAPI mjModel *Compile();
};

#endif // MUJOCO_MJWRAPPER_H_