#include "quadruped_wheel/qw_state_machine.hpp"

#ifdef USE_SIMULATION
    #define BACKWARD_HAS_DW 1
    #include "backward.hpp"
    namespace backward{
        backward::SignalHandling sh;
    }
#endif

using namespace types;
MotionStateFeedback StateBase::msfb_ = MotionStateFeedback();

int main(int argc, char **argv){
    std::cout << "State Machine Start Running" << std::endl;
    rclcpp::init(0, 0);
    RobotName robot_name = RobotName::FireAnt;
    for (int i = 1; i < argc; ++i) {
        const std::string arg(argv[i]);
        if (arg == "--robot=m20") robot_name = RobotName::M20;
        if (arg == "--robot=fire_ant") robot_name = RobotName::FireAnt;
    }
    //KeyBoard control
    std::shared_ptr<StateMachineBase> fsm = std::make_shared<qw::QwStateMachine>(robot_name, RemoteCommandType::kKeyBoard);
    //Gamepad control
    // std::shared_ptr<StateMachineBase> fsm = std::make_shared<qw::QwStateMachine>(RobotName::M20, RemoteCommandType::kGamepad);
    
    fsm->Start();
    fsm->Run();
    fsm->Stop();

    rclcpp::shutdown();
    return 0;
}