inspiredhand/tools/config_loader.cpp

#include "config_loader.h"
#include "tools.h"
#include <fstream>
#include <sstream>
#include <algorithm>
#include <string>

namespace config {

static AppConfig g_default_cfg;        // cached default config
static bool g_default_cfg_loaded = false;

static bool parse_int(const std::string &src, const std::string &key, int &out) {
    auto pos = src.find("\"" + key + "\"");
    if (pos == std::string::npos) return false;
    pos = src.find(':', pos);
    if (pos == std::string::npos) return false;
    size_t end = src.find_first_of(",}\n", pos+1);
    std::string val = tools::trim(src.substr(pos+1, end == std::string::npos ? std::string::npos : end - (pos+1)));
    try { out = std::stoi(val); return true; } catch (...) { return false; }
}

static bool parse_double(const std::string &src, const std::string &key, double &out) {
    auto pos = src.find("\"" + key + "\"");
    if (pos == std::string::npos) return false;
    pos = src.find(':', pos);
    if (pos == std::string::npos) return false;
    size_t end = src.find_first_of(",}\n", pos+1);
    std::string val = tools::trim(src.substr(pos+1, end == std::string::npos ? std::string::npos : end - (pos+1)));
    try { out = std::stod(val); return true; } catch (...) { return false; }
}

static bool parse_string(const std::string &src, const std::string &key, std::string &out) {
    auto pos = src.find("\"" + key + "\"");
    if (pos == std::string::npos) return false;
    pos = src.find(':', pos);
    if (pos == std::string::npos) return false;
    pos = src.find('"', pos);
    if (pos == std::string::npos) return false;
    size_t end = src.find('"', pos+1);
    if (end == std::string::npos) return false;
    out = src.substr(pos+1, end - (pos+1));
    return true;
}

bool load_config(const std::string &path, AppConfig &out)
{
    std::ifstream in(path.c_str());
    if (!in) return false;
    std::stringstream ss; ss << in.rdbuf();
    std::string src = ss.str();

    // top-level
    parse_string(src, "robot_ip", out.robot_ip);

    // tio section (optional): use dotted keys or nested JSON by simple search
    parse_int(src, "vout_enable", out.tio.vout_enable);
    parse_int(src, "vout_vol", out.tio.vout_vol);
    parse_int(src, "pin_mode", out.tio.pin_mode);
    parse_int(src, "rs485_channel", out.tio.rs485_channel);
    parse_int(src, "slave_id", out.tio.slave_id);
    parse_int(src, "baudrate", out.tio.baudrate);
    parse_int(src, "databit", out.tio.databit);
    parse_int(src, "stopbit", out.tio.stopbit);
    parse_int(src, "parity", out.tio.parity);

    // robot motion parameters (optional)
    parse_int(src, "joint_speed_deg_s",  out.robot.joint_speed_deg_s);
    parse_int(src, "joint_acc_deg_s2",   out.robot.joint_acc_deg_s2);
    parse_int(src, "line_speed_mm_s",    out.robot.line_speed_mm_s);
    parse_int(src, "line_acc_mm_s2",     out.robot.line_acc_mm_s2);
    parse_int(src, "pose_speed_deg_s",   out.robot.pose_speed_deg_s);
    parse_int(src, "pose_acc_deg_s2",    out.robot.pose_acc_deg_s2);

    for (int i = 0; i < 6; ++i) {
        std::string key_home = "home_joint_deg" + std::to_string(i);
        parse_double(src, key_home, out.robot.home_joint_deg[i]);
    }

    // hand initial params (optional): hand_angle0..5, hand_speed0..5, hand_force0..5
    for (int i = 0; i < 6; ++i) {
        std::string key_angle = "hand_angle" + std::to_string(i);
        std::string key_speed = "hand_speed" + std::to_string(i);
        std::string key_force = "hand_force" + std::to_string(i);
        parse_int(src, key_angle, out.hand.angle[i]);
        parse_int(src, key_speed, out.hand.speed[i]);
        parse_int(src, key_force, out.hand.force[i]);
    }

    // grasp task (optional): grasp_a_joint_deg0..5, grasp_b_joint_deg0..5, grasp_descend_mm
    for (int i = 0; i < 6; ++i) {
        std::string key_a = "grasp_a_joint_deg" + std::to_string(i);
        std::string key_b = "grasp_b_joint_deg" + std::to_string(i);
        parse_double(src, key_a, out.grasp.grasp_a.joint_deg[i]);
        parse_double(src, key_b, out.grasp.grasp_b.joint_deg[i]);
    }
    parse_double(src, "grasp_joint_speed_deg", out.grasp.grasp_joint_speed_deg);
    parse_double(src, "grasp_joint_acc_deg", out.grasp.grasp_joint_acc_deg);
    parse_double(src, "grasp_descend_mm", out.grasp.grasp_descend_mm);

    // grasp strategies (optional): grasp_strategy{N}_thumb_bend/rotation/translate
    out.grasp_strategies.clear();
    for (int i = 0; i < 32; ++i) {
        config::GraspStrategyConfig s;
        bool any = false;
        std::string key_bend = "grasp_strategy" + std::to_string(i) + "_thumb_bend";
        std::string key_rot  = "grasp_strategy" + std::to_string(i) + "_thumb_rotation";
        std::string key_trans = "grasp_strategy" + std::to_string(i) + "_thumb_translate";
        if (parse_int(src, key_bend, s.thumb_bend)) any = true;
        if (parse_int(src, key_rot,  s.thumb_rotation)) any = true;
        if (parse_int(src, key_trans, s.thumb_translate)) any = true;
        if (!any) continue;
        out.grasp_strategies.push_back(s);
    }

    return true;
}

bool load_default_config(AppConfig &out)
{
    // default: project root config.json (relative to runtime cwd)
    std::string path = "../config.json";
    return load_config(path, out);
}

const AppConfig& get_default_app_config()
{
    if (!g_default_cfg_loaded) {
        // Try to load from file; if it fails, keep struct defaults
        load_default_config(g_default_cfg);
        g_default_cfg_loaded = true;
    }
    return g_default_cfg;
}

}