#ifndef __UT_ROBOT_H2_LOCO_API_HPP__
#define __UT_ROBOT_H2_LOCO_API_HPP__

#include <unitree/common/json/jsonize.hpp>
#include <variant>

namespace unitree {
namespace robot {
namespace h2 {
/*service name*/
const std::string LOCO_SERVICE_NAME = "sport";

/*api version*/
const std::string LOCO_API_VERSION = "1.0.0.0";

/*api id*/
const int32_t ROBOT_API_ID_LOCO_GET_FSM_ID = 7001;
const int32_t ROBOT_API_ID_LOCO_GET_FSM_MODE = 7002;
const int32_t ROBOT_API_ID_LOCO_GET_BALANCE_MODE = 7003;
const int32_t ROBOT_API_ID_LOCO_GET_SWING_HEIGHT = 7004;
const int32_t ROBOT_API_ID_LOCO_GET_STAND_HEIGHT = 7005;
const int32_t ROBOT_API_ID_LOCO_GET_PHASE = 7006;  // deprecated
const int32_t ROBOT_API_ID_LOCO_GET_ARM_SDK_STATUS = 7007;
const int32_t ROBOT_API_ID_LOCO_GET_AVAILABLE_FSM_IDS = 7008;

const int32_t ROBOT_API_ID_LOCO_SET_FSM_ID = 7101;
const int32_t ROBOT_API_ID_LOCO_SET_BALANCE_MODE = 7102;
const int32_t ROBOT_API_ID_LOCO_SET_SWING_HEIGHT = 7103;
const int32_t ROBOT_API_ID_LOCO_SET_STAND_HEIGHT = 7104;
const int32_t ROBOT_API_ID_LOCO_SET_VELOCITY = 7105;
const int32_t ROBOT_API_ID_LOCO_SET_ARM_TASK = 7106;
const int32_t ROBOT_API_ID_LOCO_SET_SPEED_MODE = 7107;
const int32_t ROBOT_API_ID_LOCO_SET_PUNCH_API = 7108;
const int32_t ROBOT_API_ID_LOCO_SET_ARM_SDK_STATUS = 7109;

using LocoCmd = std::map<std::string, std::variant<int, float, std::vector<float>>>;

class JsonizeDataVecFloat : public common::Jsonize {
 public:
  JsonizeDataVecFloat() {}
  ~JsonizeDataVecFloat() {}

  void fromJson(common::JsonMap& json) { common::FromJson(json["data"], data); }

  void toJson(common::JsonMap& json) const { common::ToJson(data, json["data"]); }

  std::vector<float> data;
};

class JsonizeVelocityCommand : public common::Jsonize {
 public:
  JsonizeVelocityCommand() {}
  ~JsonizeVelocityCommand() {}

  void fromJson(common::JsonMap& json) {
    common::FromJson(json["velocity"], velocity);
    common::FromJson(json["duration"], duration);
  }

  void toJson(common::JsonMap& json) const {
    common::ToJson(velocity, json["velocity"]);
    common::ToJson(duration, json["duration"]);
  }

  std::vector<float> velocity;
  float duration;
};

struct FsmIdInfo {
  int id;
  std::string name;

  FsmIdInfo() : id(0), name("") {}
  FsmIdInfo(int i, const std::string& n) : id(i), name(n) {}
};

class JsonizeFsmIdList : public common::Jsonize {
 public:
  JsonizeFsmIdList() {}
  ~JsonizeFsmIdList() {}

  void fromJson(common::JsonMap& json) {
    std::vector<int> ids;
    std::vector<std::string> names;

    common::FromJson(json["ids"], ids);
    common::FromJson(json["names"], names);

    fsm_ids.clear();
    for (size_t i = 0; i < ids.size() && i < names.size(); i++) {
      fsm_ids.push_back(FsmIdInfo(ids[i], names[i]));
    }
  }

  void toJson(common::JsonMap& json) const {
    std::vector<int> ids;
    std::vector<std::string> names;

    for (const auto& info : fsm_ids) {
      ids.push_back(info.id);
      names.push_back(info.name);
    }

    common::ToJson(ids, json["ids"]);
    common::ToJson(names, json["names"]);
  }

  std::vector<FsmIdInfo> fsm_ids;
};

}  // namespace h2
}  // namespace robot
}  // namespace unitree

#endif // __UT_ROBOT_H2_LOCO_API_HPP__
