#pragma once

#include <fstream>
#include <string>
#include <iostream>
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "nlohmann/json.hpp"

using json = nlohmann::json;

struct RmoCtlConfig
{
    std::string serial_port;
    int baudrate;
    int mcu_rpm_max;
    double eps_angle_max;
};

bool load_config(RmoCtlConfig &config)
{
    try
    {
        std::string config_path = ament_index_cpp::get_package_share_directory("radio_ctrl") + "/config/config.json";

        std::ifstream ifs(config_path);
        if (!ifs.is_open())
        {
            std::cerr << "Failed to open config file: " << config_path << std::endl;
            return false;
        }

        json j;
        ifs >> j;

        config.serial_port = j.at("serial_port").get<std::string>();
        config.baudrate = j.at("baudrate").get<int>();
        config.mcu_rpm_max = j.at("rmoctl_para").at("mcu_rpm_max").get<int>();
        config.eps_angle_max = j.at("rmoctl_para").at("eps_angle_max").get<double>();

        return true;
    }
    catch (const std::exception &e)
    {
        std::cerr << "Error parsing config: " << e.what() << std::endl;
        return false;
    }
}