#include <rclcpp/rclcpp.hpp>
#include "interfaces/msg/input_msg.hpp"
#include "std_msgs/msg/string.hpp"
#include "input_dev/input_cfg.hpp"
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.hpp>
#include <message_filters/synchronizer.h>
#include"input_dev/input_dev_node.hpp"
#include"input_dev/key_map.hpp"
#include <vector>
#include<unordered_map>
#include<utility>
using namespace std;
using namespace input_dev;
//
std::pair<std::string, float> parseMsg(const std::string& data) {
    std::stringstream ss(data);
    std::string text, float_str;
    
    std::getline(ss, text, ',');
    std::getline(ss, float_str);
    
    try {
        float float_val = std::stof(float_str);
        return {text, float_val};
    } catch (const std::exception& e) {
        throw std::runtime_error("Failed to parse float value");
    }
}

static shared_ptr<InputSub> cur_node=nullptr;
InputCfg cfg0=InputCfg("f710", "gamepad","front","/gamepad_msg");
InputCfg cfg1=InputCfg("ifly-m2", "mic","front","/mic_msg");
int main(int argc, char* argv[]) {
    std::setlocale(LC_ALL, "en_US.UTF-8");

    rclcpp::init(argc, argv);
    cur_node=make_shared<InputSub>();
    if(!cur_node){
        RCLCPP_ERROR(rclcpp::get_logger("main"),"input_dev node create error!!!!");
        return -1;
    }
    //超声测距
    rclcpp::TimerBase::SharedPtr timer_=cur_node->create_wall_timer(std::chrono::milliseconds(100), [=]() { 
    });
    #if 1
    //f710
    auto gamepad_sub = cur_node->create_subscription<std_msgs::msg::String>(cfg0.getInputTopic(), 10,
                            [=](const std_msgs::msg::String::SharedPtr msg) { 
                                ///RCLCPP_INFO(cur_node->get_logger(), "gamepad msg: %s", msg->data.c_str());
                                auto [text, float_val] = parseMsg(msg->data);
                                ///cout<<text<<":"<<float_val<<endl;
                                int out_key=gamepad_map({text,float_val});
                                cout<<"LG OUT:"<<key_str(out_key)<<endl;
                                cur_node->pub_msg(out_key,cfg0);
                            });
    #endif
    auto mic_sub = cur_node->create_subscription<std_msgs::msg::String>(cfg1.getInputTopic(), 10,
                            [=](const std_msgs::msg::String::SharedPtr msg) { 
                                int out_key=mic_map(msg->data);
                                cout<<"KD OUT:"<<key_str(out_key)<<endl;
                                cur_node->pub_msg(out_key,cfg1);
                            });
    rclcpp::spin(cur_node); 
    rclcpp::shutdown(); 
    return 0;
}
