#ifndef GO1_PRO_UNITREE_LEGGED_LOOP_H_
#define GO1_PRO_UNITREE_LEGGED_LOOP_H_

#include <atomic>
#include <boost/function.hpp>
#include <string>
#include <thread>

namespace UNITREE_LEGGED_SDK {

constexpr int THREAD_PRIORITY = 95;
typedef boost::function<void()> Callback;

class Loop {
 public:
  Loop(std::string name, float period, int bindCPU = -1)
      : _name(name), _period(period), _bindCPU(bindCPU) {}
  virtual ~Loop();
  void start();
  void shutdown();
  virtual void functionCB() = 0;

 private:
  void entryFunc();
  std::string _name;
  float _period;
  int _bindCPU;
  bool _bind_cpu_flag = false;
  bool _isrunning = false;
  std::atomic<bool> _running_atomic{false};
  std::thread _thread;
};

class LoopFunc : public Loop {
 public:
  LoopFunc(std::string name, float period, const Callback& cb)
      : Loop(name, period), _fp(cb) {}
  LoopFunc(std::string name, float period, int bindCPU, const Callback& cb)
      : Loop(name, period, bindCPU), _fp(cb) {}
  void functionCB() override { _fp(); }

 private:
  boost::function<void()> _fp;
};

}  // namespace UNITREE_LEGGED_SDK

#endif