#pragma once

#include <memory>
#include <atomic>
#include <nlohmann/json.hpp>

#include <unitree/robot/go2/sport/sport_client.hpp>
#include <unitree/robot/go2/obstacles_avoid/obstacles_avoid_client.hpp>
#include <unitree/robot/b2/motion_switcher/motion_switcher_client.hpp>
#include <unitree/robot/go2/video/video_client.hpp>
#include <unitree/robot/go2/vui/vui_client.hpp>
#include <unitree/robot/channel/channel_subscriber.hpp>
#include <unitree/idl/go2/SportModeState_.hpp>
#include "logger.hpp"

namespace custom {



class Controller {
public:
    Controller();
    ~Controller();

    bool initialize();
    bool start();
    bool stop();
    void shutdown() { stop(); }
    bool isRunning() const { return running_; }
    
    // Sport
    bool StandUp();
    bool StandDown();
    bool Sit();
    bool Damp();
    bool RecoveryStand();
    bool StopMove();
    bool BalanceStand();
    bool Euler(float roll, float pitch, float yaw);
    bool Move(float vx, float vy, float vyaw);
    bool RiseSit();
    bool SpeedLevel(int level);
    bool Hello();
    bool Stretch();
    bool SwitchJoystick(bool flag);
    bool Heart();
    bool Pose(bool flag);
    bool Scrape();
    bool FrontFlip();
    bool FrontJump();
    bool FrontPounce();
    bool Dance1();
    bool Dance2();
    bool TrajectoryFollow(const std::vector<std::array<float, 6>>& path);
    bool FootRaiseHeight(float height);
    
    // Obstacle 
    bool SwitchSet(bool enable);
    bool SwitchGet(bool& enable);
    bool UseRemoteCommandFromApi(bool isRemoteCommandsFromApi);
    bool MoveToAbsolutePosition(float x, float y, float yaw);
    bool MoveToIncrementPosition(float x, float y, float yaw);
    
    // MotionSwitcher
    bool CheckMode(std::string& form, std::string& name);
    bool SelectMode(const std::string& nameOrAlias);
    bool ReleaseMode();

private:
    template<typename Func>
    bool ExecuteSportCmd(Func&& func);
    
    template<typename Func>
    bool ExecuteObstacleCmd(Func&& func);

    template<typename Func>
    bool ExecuteMotionSwitchCmd(Func&& func);
    
    std::unique_ptr<unitree::robot::go2::SportClient> sc_;
    std::unique_ptr<unitree::robot::go2::ObstaclesAvoidClient> oac_;
    std::unique_ptr<unitree::robot::b2::MotionSwitcherClient> msc_;
    
    std::atomic<bool> running_;
    std::atomic<bool> initialized_;
};

} // namespace custom