#ifndef GO1_PRO_UNITREE_LEGGED_UDP_H_
#define GO1_PRO_UNITREE_LEGGED_UDP_H_

#include "comm.h"
#include "unitree_legged_sdk/quadruped.h"
#include <cstdint>

namespace UNITREE_LEGGED_SDK {

constexpr int UDP_CLIENT_PORT = 8080;
constexpr int UDP_SERVER_PORT = 8007;
constexpr char UDP_SERVER_IP_BASIC[] = "192.168.123.10";
constexpr char UDP_SERVER_IP_SPORT[] = "192.168.123.161";

enum RecvEnum { nonBlock = 0x00, block = 0x01, blockTimeout = 0x02 };

class UDP {
 public:
  UDP(uint8_t level, uint16_t localPort, const char* targetIP, uint16_t targetPort);
  UDP(uint16_t localPort, const char* targetIP, uint16_t targetPort,
      int sendLength, int recvLength, bool initiativeDisconnect = false,
      RecvEnum recvType = RecvEnum::nonBlock);
  UDP(uint16_t localPort, int sendLength, int recvLength,
      bool initiativeDisconnect = false, RecvEnum recvType = RecvEnum::nonBlock,
      bool setIpPort = false);
  ~UDP();

  UDP(const UDP&) = delete;
  UDP& operator=(const UDP&) = delete;

  void SetIpPort(const char* targetIP, uint16_t targetPort);
  void SetRecvTimeout(int time);
  void SetDisconnectTime(float callback_dt, float disconnectTime);
  void SetAccessibleTime(float callback_dt, float accessibleTime);

  int Send();
  int Recv();

  void InitCmdData(HighCmd& cmd);
  void InitCmdData(LowCmd& cmd);
  int SetSend(char* data);
  int SetSend(HighCmd& cmd);
  int SetSend(LowCmd& cmd);
  void GetRecv(char* data);
  void GetRecv(HighState& state);
  void GetRecv(LowState& state);

  UDPState udpState;
  char* targetIP;
  uint16_t targetPort;
  char* localIP;
  uint16_t localPort;
  bool accessible = false;

 private:
  struct Impl;
  Impl* impl_;
};

}  // namespace UNITREE_LEGGED_SDK

#endif