#ifndef GO1_PRO_UNITREE_LEGGED_COMM_H_
#define GO1_PRO_UNITREE_LEGGED_COMM_H_

#include <array>
#include <cstdint>

namespace UNITREE_LEGGED_SDK {

constexpr int HIGHLEVEL = 0xee;
constexpr int LOWLEVEL = 0xff;
constexpr int TRIGERLEVEL = 0xf0;
constexpr double PosStopF = 2.146E+9f;
constexpr double VelStopF = 16000.0f;

extern const int HIGH_CMD_LENGTH;
extern const int HIGH_STATE_LENGTH;
extern const int LOW_CMD_LENGTH;
extern const int LOW_STATE_LENGTH;

#pragma pack(push, 1)

struct BmsCmd {
  uint8_t off;
  std::array<uint8_t, 3> reserve;
};

struct BmsState {
  uint8_t version_h;
  uint8_t version_l;
  uint8_t bms_status;
  uint8_t SOC;
  int32_t current;
  uint16_t cycle;
  std::array<int8_t, 2> BQ_NTC;
  std::array<int8_t, 2> MCU_NTC;
  std::array<uint16_t, 10> cell_vol;
};

struct Cartesian {
  float x;
  float y;
  float z;
};

struct IMU {
  std::array<float, 4> quaternion;
  std::array<float, 3> gyroscope;
  std::array<float, 3> accelerometer;
  std::array<float, 3> rpy;
  int8_t temperature;
};

struct LED {
  uint8_t r;
  uint8_t g;
  uint8_t b;
};

struct MotorState {
  uint8_t mode;
  float q;
  float dq;
  float ddq;
  float tauEst;
  float q_raw;
  float dq_raw;
  float ddq_raw;
  int8_t temperature;
  std::array<uint32_t, 2> reserve;
};

struct MotorCmd {
  uint8_t mode;
  float q;
  float dq;
  float tau;
  float Kp;
  float Kd;
  std::array<uint32_t, 3> reserve;
};

struct LowState {
  std::array<uint8_t, 2> head;
  uint8_t levelFlag;
  uint8_t frameReserve;
  std::array<uint32_t, 2> SN;
  std::array<uint32_t, 2> version;
  uint16_t bandWidth;
  IMU imu;
  std::array<MotorState, 20> motorState;
  BmsState bms;
  std::array<int16_t, 4> footForce;
  std::array<int16_t, 4> footForceEst;
  uint32_t tick;
  std::array<uint8_t, 40> wirelessRemote;
  uint32_t reserve;
  uint32_t crc;
};

struct LowCmd {
  std::array<uint8_t, 2> head;
  uint8_t levelFlag;
  uint8_t frameReserve;
  std::array<uint32_t, 2> SN;
  std::array<uint32_t, 2> version;
  uint16_t bandWidth;
  std::array<MotorCmd, 20> motorCmd;
  BmsCmd bms;
  std::array<uint8_t, 40> wirelessRemote;
  uint32_t reserve;
  uint32_t crc;
};

struct HighState {
  std::array<uint8_t, 2> head;
  uint8_t levelFlag;
  uint8_t frameReserve;
  std::array<uint32_t, 2> SN;
  std::array<uint32_t, 2> version;
  uint16_t bandWidth;
  IMU imu;
  std::array<MotorState, 20> motorState;
  BmsState bms;
  std::array<int16_t, 4> footForce;
  std::array<int16_t, 4> footForceEst;
  uint8_t mode;
  float progress;
  uint8_t gaitType;
  float footRaiseHeight;
  std::array<float, 3> position;
  float bodyHeight;
  std::array<float, 3> velocity;
  float yawSpeed;
  std::array<float, 4> rangeObstacle;
  std::array<Cartesian, 4> footPosition2Body;
  std::array<Cartesian, 4> footSpeed2Body;
  std::array<uint8_t, 40> wirelessRemote;
  uint32_t reserve;
  uint32_t crc;
};

struct HighCmd {
  std::array<uint8_t, 2> head;
  uint8_t levelFlag;
  uint8_t frameReserve;
  std::array<uint32_t, 2> SN;
  std::array<uint32_t, 2> version;
  uint16_t bandWidth;
  uint8_t mode;
  uint8_t gaitType;
  uint8_t speedLevel;
  float footRaiseHeight;
  float bodyHeight;
  std::array<float, 2> position;
  std::array<float, 3> euler;
  std::array<float, 2> velocity;
  float yawSpeed;
  BmsCmd bms;
  std::array<LED, 4> led;
  std::array<uint8_t, 40> wirelessRemote;
  uint32_t reserve;
  uint32_t crc;
};

#pragma pack(pop)

struct UDPState {
  unsigned long long TotalCount;
  unsigned long long SendCount;
  unsigned long long RecvCount;
  unsigned long long SendError;
  unsigned long long FlagError;
  unsigned long long RecvCRCError;
  unsigned long long RecvLoseError;
};

}  // namespace UNITREE_LEGGED_SDK

#endif