/**
 * engines/cpp/main.cpp
 * --------------------
 * C++ 动力学模拟引擎。
 * 与 Python 版 (compute.py) 算法保持一致。
 *
 * 输入: param.json, <input_dir>/coord.txt, connection.txt, bond.txt
 * 输出: <output_dir>/trajectory.txt (JSON, 与 Python 版兼容)
 *
 * 编译:
 *   g++ -O3 -march=native -std=c++17 -o build/dynamics_cpp main.cpp
 *
 * 用法:
 *   ./build/dynamics_cpp <input_dir> <output_dir> <param_json>
 */

#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstring>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <sstream>
#include <string>
#include <vector>

// ========================================================================
// 配置参数（从 param.json 读取）
// ========================================================================
struct SimParams {
    double box_a = 10.0;
    int NT = 10000;
    double DT = 0.001;
    int NSTEP = 100;
    int warmup_steps = 0;
    std::string method = "leapfrog";
    double G[3] = {0, 0, -9.8};
    double B[3] = {0, 0, 0};
    int gravity_field = 1;
    int gravity_interaction = 0;
    int elastic_force = 1;
    int damping_force = 0;
    double gravity_strength = 1.0;
    int driving_force = 0;
    int save_trajectory = 1;
    double alpha[6] = {0,0,0,0,0,0};
    double ball_radius = 0.5;
    double ball_color[3] = {0.9, 0.2, 0.2};
    double box_color[3] = {0.8, 0.8, 0.85};
    int use_marker = 0;
    double camera_distance = 40.0;
    double camera_elevation = 0;
    double camera_azimuth = 0;
};

// ========================================================================
// 原子数据
// ========================================================================
struct AtomData {
    std::vector<int> ids;
    std::vector<double> masses;
    std::vector<double> radii;
    std::vector<double> pos_0;   // (n_atoms * 3)
    std::vector<double> vel_0;   // (n_atoms * 3)
    std::vector<int> fixed;      // (n_atoms * 3), 0/1 flags
};

// ========================================================================
// 成键数据
// ========================================================================
struct BondData {
    std::vector<int> pairs;          // (n_bonds * 2)
    std::vector<double> stiffness;
    std::vector<double> rest_lengths;
};

// ========================================================================
// 驱动力数据
// ========================================================================
struct DriverData {
    int n_drivers = 0;
    std::vector<int> atom_idx;       // internal atom indices
    std::vector<double> amp_x, amp_y, amp_z;
    std::vector<double> freq_x, freq_y, freq_z;
    std::vector<double> phi_x, phi_y, phi_z;  // radians
    std::vector<int> has_period;     // 0=all, 1=limited
    std::vector<double> period_cycles;
    std::vector<double> freeze_x, freeze_y, freeze_z;
};

// ========================================================================
// 辅助函数
// ========================================================================

static void die(const std::string &msg) {
    std::cerr << "[C++-engine] 错误: " << msg << std::endl;
    exit(1);
}

/* 读整个文件为字符串 */
static std::string read_file(const std::string &path) {
    std::ifstream f(path, std::ios::binary);
    if (!f) die("无法打开 " + path);
    std::ostringstream ss;
    ss << f.rdbuf();
    return ss.str();
}

/* 从 JSON 中查找 key，返回冒号后的数值 */
static double json_read_double(const std::string &json, const std::string &key) {
    auto pos = json.find("\"" + key + "\"");
    if (pos == std::string::npos) return 0.0;
    pos = json.find(':', pos);
    if (pos == std::string::npos) return 0.0;
    while (pos < json.size() && (json[pos] == ':' || json[pos] == ' ' || json[pos] == '\t' || json[pos] == '\n')) pos++;
    return std::stod(json.substr(pos));
}

static int json_read_int(const std::string &json, const std::string &key) {
    return static_cast<int>(json_read_double(json, key));
}

/* 从 JSON 中读取字符串值 */
static std::string json_read_string(const std::string &json, const std::string &key) {
    auto pos = json.find("\"" + key + "\"");
    if (pos == std::string::npos) return "";
    pos = json.find(':', pos);
    if (pos == std::string::npos) return "";
    while (pos < json.size() && (json[pos] == ':' || json[pos] == ' ' || json[pos] == '\t' || json[pos] == '\n')) pos++;
    if (pos >= json.size()) return "";
    // 找到引号
    if (json[pos] != '"') return "";
    pos++;
    std::string result;
    while (pos < json.size() && json[pos] != '"') {
        result += json[pos];
        pos++;
    }
    return result;
}

/* 检查 JSON 中是否存在某个 key */
static bool json_has_key(const std::string &json, const std::string &key) {
    return json.find("\"" + key + "\"") != std::string::npos;
}

/* 读取 JSON 数组 (如 "G": [0, 0, -9.8]) 到 double[3] */
static void json_read_double3(const std::string &json, const std::string &key, double out[3]) {
    auto pos = json.find("\"" + key + "\"");
    if (pos == std::string::npos) { out[0] = out[1] = out[2] = 0; return; }
    pos = json.find('[', pos);
    if (pos == std::string::npos) { out[0] = out[1] = out[2] = 0; return; }
    pos++;
    for (int i = 0; i < 3; i++) {
        while (pos < json.size() && (json[pos] == ' ' || json[pos] == '\t' || json[pos] == '\n' || json[pos] == ',')) pos++;
        char *end;
        out[i] = std::strtod(json.c_str() + pos, &end);
        pos = end - json.c_str();
    }
}

/* 读取 JSON 数组到 double[6] */
static void json_read_double6(const std::string &json, const std::string &key, double out[6]) {
    auto pos = json.find("\"" + key + "\"");
    if (pos == std::string::npos) { for (int i=0;i<6;i++) out[i]=0; return; }
    pos = json.find('[', pos);
    if (pos == std::string::npos) { for (int i=0;i<6;i++) out[i]=0; return; }
    pos++;
    for (int i = 0; i < 6; i++) {
        while (pos < json.size() && (json[pos]==' '||json[pos]=='\t'||json[pos]=='\n'||json[pos]==','||json[pos]==']')) pos++;
        char *end;
        out[i] = std::strtod(json.c_str() + pos, &end);
        pos = end - json.c_str();
    }
}

/* 解析 param.json */
static SimParams read_params(const std::string &path) {
    std::string buf = read_file(path);
    SimParams p;
    p.box_a        = json_read_double(buf, "box_a");
    p.NT           = json_read_int(buf, "NT");
    p.DT           = json_read_double(buf, "DT");
    p.NSTEP        = json_read_int(buf, "NSTEP");
    p.warmup_steps = json_read_int(buf, "warmup_steps");
    std::string m  = json_read_string(buf, "method");
    if (!m.empty()) p.method = m;
    json_read_double3(buf, "G", p.G);
    json_read_double3(buf, "B", p.B);
    p.gravity_field       = json_read_int(buf, "gravity_field");
    p.gravity_interaction = json_read_int(buf, "gravity_interaction");
    p.elastic_force       = json_read_int(buf, "elastic_force");
    p.damping_force       = json_read_int(buf, "damping_force");
    p.gravity_strength    = json_read_double(buf, "gravity_strength");
    p.driving_force       = json_read_int(buf, "driving_force");
    // save_trajectory 默认 1（全量），仅在 JSON 中存在该 key 时覆盖
    if (json_has_key(buf, "save_trajectory"))
        p.save_trajectory = json_read_int(buf, "save_trajectory");
    json_read_double6(buf, "alpha", p.alpha);
    p.ball_radius         = json_read_double(buf, "ball_radius");
    json_read_double3(buf, "ball_color", p.ball_color);
    json_read_double3(buf, "box_color", p.box_color);
    p.use_marker          = json_read_int(buf, "use_marker");
    p.camera_distance     = json_read_double(buf, "camera_distance");
    p.camera_elevation    = json_read_double(buf, "camera_elevation");
    p.camera_azimuth      = json_read_double(buf, "camera_azimuth");
    return p;
}

/* 读取 coord.txt */
static AtomData read_coord(const std::string &input_dir) {
    std::string path = input_dir + "/coord.txt";
    std::ifstream f(path);
    if (!f) die("无法打开 " + path);

    std::string header;
    std::getline(f, header);  // 跳过表头

    AtomData a;
    int id, fx, fy, fz;
    double mass, rad, px, py, pz, vx, vy, vz;
    std::string line;

    while (std::getline(f, line)) {
        if (line.empty() || line[0] == '#') continue;
        int n_parsed = std::sscanf(line.c_str(), "%d %lf %lf %lf %lf %lf %lf %lf %lf %d %d %d",
                                    &id, &mass, &rad, &px, &py, &pz, &vx, &vy, &vz, &fx, &fy, &fz);
        if (n_parsed == 9) {
            fx = fy = fz = 0;
        } else if (n_parsed != 12) {
            continue;
        }
        a.ids.push_back(id);
        a.masses.push_back(mass);
        a.radii.push_back(rad);
        a.pos_0.push_back(px); a.pos_0.push_back(py); a.pos_0.push_back(pz);
        a.vel_0.push_back(vx); a.vel_0.push_back(vy); a.vel_0.push_back(vz);
        a.fixed.push_back(fx); a.fixed.push_back(fy); a.fixed.push_back(fz);
    }

    if (a.ids.empty()) die("coord.txt 中没有原子数据");
    return a;
}

/* 读取 connection.txt 和 bond.txt */
static BondData read_bonds(const std::string &input_dir) {
    BondData b;
    std::string conn_path = input_dir + "/connection.txt";
    std::ifstream f(conn_path);
    if (!f) return b;  // 无成键

    std::string header;
    std::getline(f, header);  // 跳过表头

    // 先读取 bond.txt 获得键参数映射
    std::string bond_path = input_dir + "/bond.txt";
    std::ifstream fb(bond_path);

    int a1, a2;
    std::string bond_name;
    std::vector<std::tuple<int, int, std::string>> conn_lines;
    while (f >> a1 >> a2 >> bond_name) {
        conn_lines.emplace_back(a1 - 1, a2 - 1, bond_name);
    }

    for (auto &[i, j, name] : conn_lines) {
        double k = 1.0, r0 = 2.0;
        if (fb) {
            fb.clear();
            fb.seekg(0);
            std::string bn, header;
            double bk, br;
            std::getline(fb, header);  // 跳过表头行
            while (fb >> bn >> bk >> br) {
                if (bn == name) {
                    k = bk;
                    r0 = br;
                    break;
                }
            }
        }
        b.pairs.push_back(i);
        b.pairs.push_back(j);
        b.stiffness.push_back(k);
        b.rest_lengths.push_back(r0);
    }

    return b;
}

/* 读取 driver.txt */
static DriverData read_driver(const std::string &input_dir, const AtomData &atoms) {
    DriverData d;
    std::string path = input_dir + "/driver.txt";
    std::ifstream f(path);
    if (!f) { std::cerr << "[C++-engine] 警告: 无法打开 " << path << std::endl; return d; }

    std::string header;
    std::getline(f, header);  // skip header

    int n;
    double ax, ay, az, fx, fy, fz, px, py, pz;
    std::string period_str;

    while (f >> n >> ax >> ay >> az >> fx >> fy >> fz >> px >> py >> pz >> period_str) {
        // Find atom index by id
        int idx = -1;
        for (size_t i = 0; i < atoms.ids.size(); i++) {
            if (atoms.ids[i] == n) { idx = i; break; }
        }
        if (idx < 0) {
            std::cerr << "[C++-engine] 警告: driver.txt 原子 " << n << " 不在 coord.txt 中" << std::endl;
            continue;
        }
        d.atom_idx.push_back(idx);
        d.amp_x.push_back(ax); d.amp_y.push_back(ay); d.amp_z.push_back(az);
        d.freq_x.push_back(fx); d.freq_y.push_back(fy); d.freq_z.push_back(fz);
        // Convert degrees to radians
        const double DEG2RAD = M_PI / 180.0;
        d.phi_x.push_back(px * DEG2RAD);
        d.phi_y.push_back(py * DEG2RAD);
        d.phi_z.push_back(pz * DEG2RAD);

        if (period_str == "all") {
            d.has_period.push_back(0);
            d.period_cycles.push_back(-1.0);
        } else {
            d.has_period.push_back(1);
            d.period_cycles.push_back(std::stod(period_str));
        }
        d.freeze_x.push_back(0.0);
        d.freeze_y.push_back(0.0);
        d.freeze_z.push_back(0.0);
        d.n_drivers++;
    }

    if (d.n_drivers > 0)
        std::cout << "[C++-engine] 已加载驱动力: " << d.n_drivers << " 条定义" << std::endl;
    return d;
}

// ========================================================================
// 物理核心
// ========================================================================

/* 加速度计算（各力独立开关控制）——与 Python compute_acceleration 一致 */
static void compute_acceleration(
    int n,
    const double *x, const double *y, const double *z,
    const double *vx, const double *vy, const double *vz,
    const double *m, const double G[3], const double B[3],
    const BondData &bonds,
    int gravity_field, int gravity_interaction,
    int elastic_force, int damping_force,
    double gravity_strength,
    double *ax, double *ay, double *az)
{
    // 清零
    std::fill(ax, ax + n, 0.0);
    std::fill(ay, ay + n, 0.0);
    std::fill(az, az + n, 0.0);

    // 均匀重力场
    if (gravity_field) {
        for (int i = 0; i < n; i++) {
            ax[i] += G[0];
            ay[i] += G[1];
            az[i] += G[2];
        }
    }

    // 阻尼
    if (damping_force) {
        for (int i = 0; i < n; i++) {
            ax[i] -= B[0] * vx[i] / m[i];
            ay[i] -= B[1] * vy[i] / m[i];
            az[i] -= B[2] * vz[i] / m[i];
        }
    }

    // 弹簧力
    if (elastic_force) {
        int nb = static_cast<int>(bonds.stiffness.size());
        for (int b = 0; b < nb; b++) {
            int i = bonds.pairs[b * 2];
            int j = bonds.pairs[b * 2 + 1];
            double dx = x[j] - x[i];
            double dy = y[j] - y[i];
            double dz = z[j] - z[i];
            double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
            if (dist < 1e-12) continue;
            double stretch = dist - bonds.rest_lengths[b];
            double fmag = bonds.stiffness[b] * stretch;
            double ux = dx / dist, uy = dy / dist, uz = dz / dist;
            double fx = fmag * ux, fy = fmag * uy, fz = fmag * uz;
            ax[i] += fx / m[i];  ay[i] += fy / m[i];  az[i] += fz / m[i];
            ax[j] -= fx / m[j];  ay[j] -= fy / m[j];  az[j] -= fz / m[j];
        }
    }

    // 万有引力（所有原子对之间）
    if (gravity_interaction) {
        for (int i = 0; i < n; i++) {
            for (int j = i + 1; j < n; j++) {
                double dx = x[j] - x[i];
                double dy = y[j] - y[i];
                double dz = z[j] - z[i];
                double r2 = dx * dx + dy * dy + dz * dz;
                if (r2 <= 1e-12) continue;
                double r = std::sqrt(r2);
                double f_mag = gravity_strength * m[i] * m[j] / r2;
                double fx_g = f_mag * dx / r;
                double fy_g = f_mag * dy / r;
                double fz_g = f_mag * dz / r;
                ax[i] += fx_g / m[i]; ay[i] += fy_g / m[i]; az[i] += fz_g / m[i];
                ax[j] -= fx_g / m[j]; ay[j] -= fy_g / m[j]; az[j] -= fz_g / m[j];
            }
        }
    }
}

/* 保守力加速度（不含阻尼），供真蛙跳法专用。
   传入 damping_force=0 使 compute_acceleration 跳过阻尼项。 */
static void compute_accel_conservative(
    int n,
    const double *x, const double *y, const double *z,
    const double *m, const double G[3],
    const BondData &bonds,
    int gravity_field, int gravity_interaction,
    int elastic_force, double gravity_strength,
    double *ax, double *ay, double *az)
{
    std::vector<double> v0(n, 0.0);
    double Bzero[3] = {0.0, 0.0, 0.0};
    compute_acceleration(n, x, y, z, v0.data(), v0.data(), v0.data(),
                         m, G, Bzero, bonds,
                         gravity_field, gravity_interaction,
                         elastic_force, 0, gravity_strength,
                         ax, ay, az);
}

/* 边界条件：clamp 位置 + 速度反转 ——与 Python Limit_in_box 一致 */
static void limit_in_box(double &pos, double &vel, double lo, double hi) {
    if (pos > hi)  { pos = hi;  vel = -vel; }
    if (pos < lo)  { pos = lo;  vel = -vel; }
}

/* 周期边界回绕（与 Python wrap_position 一致）*/
static void wrap_position(double &pos, double lo, double hi) {
    if (pos > hi) pos = lo;
    if (pos < lo) pos = hi;
}

// ========================================================================
// 四种积分方法（只做位置/速度更新，不含边界条件）
// 与 Python: Explicit_Euler_Method / Implicit_Euler_Method /
//            Midpoint_Method / Leapfrog_Method 保持一致
// ========================================================================

/* ── 显式欧拉法 ──────────── */
static void explicit_euler_step(
    int n, double *x, double *y, double *z,
    double *vx, double *vy, double *vz,
    const double *m, const double G[3], const double B[3],
    const BondData &bonds, const int *fixed, double dt,
    int gravity_field, int gravity_interaction,
    int elastic_force, int damping_force,
    double gravity_strength)
{
    std::vector<double> ax(n), ay(n), az(n);
    compute_acceleration(n, x, y, z, vx, vy, vz, m, G, B, bonds,
                         gravity_field, gravity_interaction,
                         elastic_force, damping_force, gravity_strength,
                         ax.data(), ay.data(), az.data());
    for (int i = 0; i < n; i++) {
        if (fixed[i*3+0] && fixed[i*3+1] && fixed[i*3+2]) continue;
        x[i]  += vx[i] * dt;
        y[i]  += vy[i] * dt;
        z[i]  += vz[i] * dt;
        vx[i] += ax[i] * dt;
        vy[i] += ay[i] * dt;
        vz[i] += az[i] * dt;
    }
}

/* ── 隐式欧拉法 ──────────── */
static void implicit_euler_step(
    int n, double *x, double *y, double *z,
    double *vx, double *vy, double *vz,
    const double *m, const double G[3], const double B[3],
    const BondData &bonds, const int *fixed, double dt,
    int gravity_field, int gravity_interaction,
    int elastic_force, int damping_force,
    double gravity_strength)
{
    std::vector<double> ax(n), ay(n), az(n);
    for (int i = 0; i < n; i++) {
        if (fixed[i*3+0] && fixed[i*3+1] && fixed[i*3+2]) {
            ax[i] = ay[i] = az[i] = 0;
            continue;
        }
        double gamma_x = B[0] / m[i];
        double gamma_y = B[1] / m[i];
        double gamma_z = B[2] / m[i];
        // 隐式更新速度（重力 + 阻尼）
        double vxn = (vx[i] + G[0] * dt) / (1.0 + gamma_x * dt);
        double vyn = (vy[i] + G[1] * dt) / (1.0 + gamma_y * dt);
        double vzn = (vz[i] + G[2] * dt) / (1.0 + gamma_z * dt);
        // 用隐式速度 + 当前位置算加速度（包含各力开关）
        // 注意：Python 中 compute_acceleration(x, y, z, vx_next, ...) 用新速度+旧位置
        double tpx = x[i], tpy = y[i], tpz = z[i];
        compute_acceleration(1, &tpx, &tpy, &tpz, &vxn, &vyn, &vzn,
                             &m[i], G, B, bonds,
                             gravity_field, gravity_interaction,
                             elastic_force, damping_force, gravity_strength,
                             &ax[i], &ay[i], &az[i]);
        vx[i] += ax[i] * dt;
        vy[i] += ay[i] * dt;
        vz[i] += az[i] * dt;
        x[i]  += vx[i] * dt;
        y[i]  += vy[i] * dt;
        z[i]  += vz[i] * dt;
    }
}

/* ── 中点法 ──────────── */
static void midpoint_step(
    int n, double *x, double *y, double *z,
    double *vx, double *vy, double *vz,
    const double *m, const double G[3], const double B[3],
    const BondData &bonds, const int *fixed, double dt,
    int gravity_field, int gravity_interaction,
    int elastic_force, int damping_force,
    double gravity_strength)
{
    std::vector<double> ax(n), ay(n), az(n);
    compute_acceleration(n, x, y, z, vx, vy, vz, m, G, B, bonds,
                         gravity_field, gravity_interaction,
                         elastic_force, damping_force, gravity_strength,
                         ax.data(), ay.data(), az.data());
    for (int i = 0; i < n; i++) {
        if (fixed[i*3+0] && fixed[i*3+1] && fixed[i*3+2]) continue;
        double x_mid = x[i] + 0.5 * vx[i] * dt;
        double y_mid = y[i] + 0.5 * vy[i] * dt;
        double z_mid = z[i] + 0.5 * vz[i] * dt;
        double vx_mid = vx[i] + 0.5 * ax[i] * dt;
        double vy_mid = vy[i] + 0.5 * ay[i] * dt;
        double vz_mid = vz[i] + 0.5 * az[i] * dt;
        x[i] += vx_mid * dt;
        y[i] += vy_mid * dt;
        z[i] += vz_mid * dt;
        double ax_mid, ay_mid, az_mid;
        compute_acceleration(1, &x_mid, &y_mid, &z_mid, &vx_mid, &vy_mid, &vz_mid,
                             &m[i], G, B, bonds,
                             gravity_field, gravity_interaction,
                             elastic_force, damping_force, gravity_strength,
                             &ax_mid, &ay_mid, &az_mid);
        vx[i] += ax_mid * dt;
        vy[i] += ay_mid * dt;
        vz[i] += az_mid * dt;
    }
}

/* ── 蛙跳法（Velocity-Verlet）——与 Python Leapfrog_Method 一致 ── */
/* 真蛙跳一步：x(t), v(t-dt/2) → x(t+dt), v(t+dt/2)
 *
 * 无阻尼：纯保守蛙跳，每步 1 次力计算，辛积分器。
 *   v(t+dt/2) = v(t-dt/2) + a_c(t)·dt
 *
 * 有阻尼：半隐式处理，仍 1 次力计算，对任意阻尼无条件稳定。
 *   v(t+dt/2) = [v(t-dt/2)·(1-α) + a_c(t)·dt] / (1+α)，α = B·dt/(2m)
 */
static void leapfrog_full_step(
    int n, double *x, double *y, double *z,
    double *vx, double *vy, double *vz,
    const double *m, const double G[3], const double B[3],
    const BondData &bonds, const int *fixed, double dt,
    int gravity_field, int gravity_interaction,
    int elastic_force, int damping_force,
    double gravity_strength)
{
    std::vector<double> ax(n), ay(n), az(n);

    // 1 次保守力计算（不含阻尼）
    compute_accel_conservative(n, x, y, z, m, G, bonds,
                               gravity_field, gravity_interaction,
                               elastic_force, gravity_strength,
                               ax.data(), ay.data(), az.data());

    bool has_damping = damping_force && (B[0] != 0.0 || B[1] != 0.0 || B[2] != 0.0);
    for (int i = 0; i < n; i++) {
        if (fixed[i*3+0] && fixed[i*3+1] && fixed[i*3+2]) continue;
        if (has_damping) {
            double alphax = B[0] * dt / (2.0 * m[i]);
            double alphay = B[1] * dt / (2.0 * m[i]);
            double alphaz = B[2] * dt / (2.0 * m[i]);
            vx[i] = (vx[i] * (1.0 - alphax) + ax[i] * dt) / (1.0 + alphax);
            vy[i] = (vy[i] * (1.0 - alphay) + ay[i] * dt) / (1.0 + alphay);
            vz[i] = (vz[i] * (1.0 - alphaz) + az[i] * dt) / (1.0 + alphaz);
        } else {
            vx[i] += ax[i] * dt;
            vy[i] += ay[i] * dt;
            vz[i] += az[i] * dt;
        }
        x[i] += vx[i] * dt;
        y[i] += vy[i] * dt;
        z[i] += vz[i] * dt;
    }
}

/* ── 分发器：调用对应积分方法 + 边界条件（与 Python apply_motion_update 一致）── */
static void apply_step(
    const std::string &method,
    int n, double *x, double *y, double *z,
    double *vx, double *vy, double *vz,
    const double *m, const double G[3], const double B[3],
    const BondData &bonds, const int *fixed,
    const double *pos_0,
    double box_a, double dt,
    int gravity_field, int gravity_interaction,
    int elastic_force, int damping_force,
    double gravity_strength)
{
    // 积分
    if (method == "explicit_euler") {
        explicit_euler_step(n, x, y, z, vx, vy, vz, m, G, B, bonds, fixed, dt,
                            gravity_field, gravity_interaction,
                            elastic_force, damping_force, gravity_strength);
    } else if (method == "implicit_euler") {
        implicit_euler_step(n, x, y, z, vx, vy, vz, m, G, B, bonds, fixed, dt,
                            gravity_field, gravity_interaction,
                            elastic_force, damping_force, gravity_strength);
    } else if (method == "midpoint") {
        midpoint_step(n, x, y, z, vx, vy, vz, m, G, B, bonds, fixed, dt,
                      gravity_field, gravity_interaction,
                      elastic_force, damping_force, gravity_strength);
    } else if (method == "leapfrog") {
        leapfrog_full_step(n, x, y, z, vx, vy, vz, m, G, B, bonds, fixed, dt,
                           gravity_field, gravity_interaction,
                           elastic_force, damping_force, gravity_strength);
    } else {
        die("未知算法: " + method);
    }

    // 边界条件（与 Python Limit_in_box 一致）
    for (int i = 0; i < n; i++) {
        if (fixed[i*3+0] && fixed[i*3+1] && fixed[i*3+2]) continue;
        limit_in_box(x[i],  vx[i], -box_a, box_a);
        limit_in_box(y[i],  vy[i], -box_a, box_a);
        limit_in_box(z[i],  vz[i], -box_a, box_a);
    }

    // 周期边界回绕（与 Python wrap_position 一致）
    for (int i = 0; i < n; i++) {
        wrap_position(x[i], -box_a, box_a);
        wrap_position(y[i], -box_a, box_a);
        wrap_position(z[i], -box_a, box_a);
    }

    // 逐自由度固定约束（与 Python apply_fixed_constraints 一致）
    for (int i = 0; i < n; i++) {
        if (fixed[i*3+0]) { x[i] = pos_0[i*3];   vx[i] = 0.0; }
        if (fixed[i*3+1]) { y[i] = pos_0[i*3+1]; vy[i] = 0.0; }
        if (fixed[i*3+2]) { z[i] = pos_0[i*3+2]; vz[i] = 0.0; }
    }
}

// ========================================================================
// 驱动力应用
// ========================================================================

static void apply_driving_force(
    int n, double *x, double *y, double *z,
    double *vx, double *vy, double *vz,
    double t, int step, double dt,
    DriverData &drivers)
{
    if (drivers.n_drivers == 0) return;
    for (int d = 0; d < drivers.n_drivers; d++) {
        int idx = drivers.atom_idx[d];

        // Check period limits
        if (drivers.has_period[d]) {
            double max_freq = std::max({std::fabs(drivers.freq_x[d]),
                                        std::fabs(drivers.freq_y[d]),
                                        std::fabs(drivers.freq_z[d])});
            int period_steps = 0;
            if (max_freq > 1e-12) {
                period_steps = static_cast<int>(drivers.period_cycles[d] / max_freq / dt);
            }
            if (step > period_steps) {
                // Frozen: keep last position, zero velocity
                x[idx] = drivers.freeze_x[d];
                y[idx] = drivers.freeze_y[d];
                z[idx] = drivers.freeze_z[d];
                vx[idx] = vy[idx] = vz[idx] = 0.0;
                continue;
            }
        }

        const double TWO_PI = 2.0 * M_PI;
        double px = drivers.amp_x[d] * std::cos(TWO_PI * drivers.freq_x[d] * t + drivers.phi_x[d]);
        double py = drivers.amp_y[d] * std::cos(TWO_PI * drivers.freq_y[d] * t + drivers.phi_y[d]);
        double pz = drivers.amp_z[d] * std::cos(TWO_PI * drivers.freq_z[d] * t + drivers.phi_z[d]);
        double vpx = -drivers.amp_x[d] * TWO_PI * drivers.freq_x[d] * std::sin(TWO_PI * drivers.freq_x[d] * t + drivers.phi_x[d]);
        double vpy = -drivers.amp_y[d] * TWO_PI * drivers.freq_y[d] * std::sin(TWO_PI * drivers.freq_y[d] * t + drivers.phi_y[d]);
        double vpz = -drivers.amp_z[d] * TWO_PI * drivers.freq_z[d] * std::sin(TWO_PI * drivers.freq_z[d] * t + drivers.phi_z[d]);

        x[idx] = px; y[idx] = py; z[idx] = pz;
        vx[idx] = vpx; vy[idx] = vpy; vz[idx] = vpz;

        // Record freeze position at the last driving step
        if (drivers.has_period[d]) {
            double max_freq = std::max({std::fabs(drivers.freq_x[d]),
                                        std::fabs(drivers.freq_y[d]),
                                        std::fabs(drivers.freq_z[d])});
            int period_steps = 0;
            if (max_freq > 1e-12) {
                period_steps = static_cast<int>(drivers.period_cycles[d] / max_freq / dt);
            }
            if (step == period_steps) {
                drivers.freeze_x[d] = px;
                drivers.freeze_y[d] = py;
                drivers.freeze_z[d] = pz;
            }
        }
    }
}

// ========================================================================
// display.txt 输出（save_trajectory=0 时使用）
// ========================================================================

static void write_display_txt(
    const std::string &path,
    const std::vector<double> &x, const std::vector<double> &y,
    const std::vector<double> &z, const std::vector<double> &vx,
    const std::vector<double> &vy, const std::vector<double> &vz,
    int n_steps, int n_atoms,
    const SimParams &params, const AtomData &atoms)
{
    std::ofstream f(path);
    if (!f) die("无法写入 " + path);
    std::cout << "[Cpp-engine] 正在写入显示数据…" << std::endl;

    int dynamic_steps = params.NT - params.warmup_steps;
    double T_total = dynamic_steps * params.DT;

    /* number of frames 写总积分步数（与 draw.py NT 对应），不是采样帧数 */
    f << "number of frames: " << dynamic_steps << "\n";
    f << "number of particles: " << n_atoms << "\n";
    f << "DT: " << params.DT << "\n";
    f << "NSTEP: " << params.NSTEP << "\n";
    f << "method: " << params.method << "\n";
    f << "warmup_steps: " << params.warmup_steps << "\n";
    f << "dynamic_steps: " << dynamic_steps << "\n";
    f << "T_total: " << T_total << "\n";
    f << "box_a: " << params.box_a << "\n";
    f << "driving_force: " << params.driving_force << "\n";
    f << "alpha: " << params.alpha[0] << "," << params.alpha[1] << "," << params.alpha[2] << ","
                   << params.alpha[3] << "," << params.alpha[4] << "," << params.alpha[5] << "\n";
    f << "ball_radius: " << params.ball_radius << "\n";
    f << "ball_color_r: " << params.ball_color[0] << "\n";
    f << "ball_color_g: " << params.ball_color[1] << "\n";
    f << "ball_color_b: " << params.ball_color[2] << "\n";
    f << "box_color_r: " << params.box_color[0] << "\n";
    f << "box_color_g: " << params.box_color[1] << "\n";
    f << "box_color_b: " << params.box_color[2] << "\n";
    f << "use_marker: " << params.use_marker << "\n";
    f << "camera_distance: " << params.camera_distance << "\n";
    f << "camera_elevation: " << params.camera_elevation << "\n";
    f << "camera_azimuth: " << params.camera_azimuth << "\n\n";

    f << std::fixed << std::setprecision(6);
    for (int t = 0; t < n_steps; t++) {
        f << "frame:  " << (t + 1) << "\n";
        f << "n   x            y            z           vx         vy         vz\n";
        for (int i = 0; i < n_atoms; i++) {
            int base = t * n_atoms + i;
            f << std::setw(3) << atoms.ids[i]
              << std::setw(12) << x[base]
              << std::setw(12) << y[base]
              << std::setw(12) << z[base]
              << std::setw(12) << vx[base]
              << std::setw(12) << vy[base]
              << std::setw(12) << vz[base] << "\n";
        }
    }
}

// ========================================================================
// JSON 输出
// ========================================================================

static void write_trajectory_json(
    const std::string &path,
    const std::vector<double> &x, const std::vector<double> &y,
    const std::vector<double> &z, const std::vector<double> &vx,
    const std::vector<double> &vy, const std::vector<double> &vz,
    int n_steps, int n_atoms,
    const SimParams &params, const AtomData &atoms, const BondData &bonds)
{
    std::ofstream f(path);
    if (!f) die("无法写入 " + path);
    std::cout << "[Cpp-engine] 正在写入轨迹数据…" << std::endl;
    f << std::setprecision(8);

    f << "{\n";

    // 轨迹数组
    const std::string names[] = {"traj_x","traj_y","traj_z","traj_vx","traj_vy","traj_vz"};
    const std::vector<double> *arrs[] = {&x, &y, &z, &vx, &vy, &vz};

    for (int a = 0; a < 6; a++) {
        f << "  \"" << names[a] << "\": [\n";
        const auto &data = *arrs[a];
        for (int t = 0; t < n_steps; t++) {
            f << "    [";
            for (int i = 0; i < n_atoms; i++) {
                f << data[t * n_atoms + i];
                if (i < n_atoms - 1) f << ',';
            }
            f << ']';
            if (t < n_steps - 1) f << ',';
            f << '\n';
        }
        f << "  ],\n";
    }

    // 标量参数
    f << "  \"NT\": " << params.NT << ",\n";
    f << "  \"DT\": " << params.DT << ",\n";
    f << "  \"NSTEP\": " << params.NSTEP << ",\n";
    f << "  \"method\": \"" << params.method << "\",\n";
    f << "  \"warmup_steps\": " << params.warmup_steps << ",\n";
    f << "  \"G\": [" << params.G[0] << ", " << params.G[1] << ", " << params.G[2] << "],\n";
    f << "  \"B\": [" << params.B[0] << ", " << params.B[1] << ", " << params.B[2] << "],\n";

    // 原子信息
    f << "  \"atom_ids\": [";
    for (size_t i = 0; i < atoms.ids.size(); i++) {
        if (i > 0) f << ',';
        f << atoms.ids[i];
    }
    f << "],\n";

    f << "  \"atom_masses\": [";
    for (size_t i = 0; i < atoms.masses.size(); i++) {
        if (i > 0) f << ',';
        f << atoms.masses[i];
    }
    f << "],\n";

    // 成键
    f << "  \"bond_pairs\": [";
    for (size_t b = 0; b < bonds.stiffness.size(); b++) {
        if (b > 0) f << ',';
        f << "[" << bonds.pairs[b * 2] << ", " << bonds.pairs[b * 2 + 1] << "]";
    }
    f << "],\n";

    f << "  \"bond_stiffness\": [";
    for (size_t b = 0; b < bonds.stiffness.size(); b++) {
        if (b > 0) f << ',';
        f << bonds.stiffness[b];
    }
    f << "],\n";

    f << "  \"bond_rest_lengths\": [";
    for (size_t b = 0; b < bonds.rest_lengths.size(); b++) {
        if (b > 0) f << ',';
        f << bonds.rest_lengths[b];
    }
    f << "],\n";

    f << "  \"driving_force\": " << params.driving_force << "\n";

    f << "}\n";
}

// ========================================================================
// 主函数
// ========================================================================

int main(int argc, char **argv) {
    if (argc < 4) {
        std::cerr << "用法: " << argv[0] << " <input_dir> <output_dir> <param_json>" << std::endl;
        return 1;
    }

    std::string input_dir  = argv[1];
    std::string output_dir = argv[2];
    std::string param_path = argv[3];

    auto t0 = std::chrono::high_resolution_clock::now();

    // 读取参数和输入
    SimParams params = read_params(param_path);
    AtomData  atoms  = read_coord(input_dir);
    BondData  bonds  = read_bonds(input_dir);

    DriverData drivers;
    if (params.driving_force) {
        drivers = read_driver(input_dir, atoms);
    }

    std::cout << "[C++-engine] 原子数=" << atoms.ids.size()
              << ", 键数=" << bonds.stiffness.size()
              << ", NT=" << params.NT << ", DT=" << params.DT
              << ", method=" << params.method << std::endl;

    int n = static_cast<int>(atoms.ids.size());

    // 初始化位置/速度
    std::vector<double> x(n), y(n), z(n), vx(n), vy(n), vz(n);
    for (int i = 0; i < n; i++) {
        x[i]  = atoms.pos_0[i * 3];
        y[i]  = atoms.pos_0[i * 3 + 1];
        z[i]  = atoms.pos_0[i * 3 + 2];
        vx[i] = atoms.vel_0[i * 3];
        vy[i] = atoms.vel_0[i * 3 + 1];
        vz[i] = atoms.vel_0[i * 3 + 2];
    }

    // 分配轨迹缓冲区
    int record_steps = params.NT - params.warmup_steps;
    int nstep_sampling = (params.save_trajectory == 0) ? params.NSTEP : 1;
    int buf_steps = (params.save_trajectory == 0) ? (record_steps / nstep_sampling) : record_steps;
    std::vector<double> traj_x(buf_steps * n);
    std::vector<double> traj_y(buf_steps * n);
    std::vector<double> traj_z(buf_steps * n);
    std::vector<double> traj_vx(buf_steps * n);
    std::vector<double> traj_vy(buf_steps * n);
    std::vector<double> traj_vz(buf_steps * n);

    // 真蛙跳初始化：v(0) 反推 v(-dt/2) = v(0) - 0.5*a_c(0)*dt
    if (params.method == "leapfrog") {
        std::vector<double> ax0(n), ay0(n), az0(n);
        compute_accel_conservative(n, x.data(), y.data(), z.data(), atoms.masses.data(), params.G, bonds,
                                   params.gravity_field, params.gravity_interaction,
                                   params.elastic_force, params.gravity_strength,
                                   ax0.data(), ay0.data(), az0.data());
        for (int i = 0; i < n; i++) {
            if (atoms.fixed[i*3+0] && atoms.fixed[i*3+1] && atoms.fixed[i*3+2]) continue;
            vx[i] -= 0.5 * ax0[i] * params.DT;
            vy[i] -= 0.5 * ay0[i] * params.DT;
            vz[i] -= 0.5 * az0[i] * params.DT;
        }
    }

    // 预热
    // 初始时刻 t=0 驱动力（与 Python run_simulation 一致）
    if (params.driving_force)
        apply_driving_force(n, x.data(), y.data(), z.data(), vx.data(), vy.data(), vz.data(), 0.0, 0, params.DT, drivers);

    for (int s = 0; s < params.warmup_steps; s++) {
        double tw = (s + 1) * params.DT;
        if (params.driving_force)
            apply_driving_force(n, x.data(), y.data(), z.data(), vx.data(), vy.data(), vz.data(), tw, s, params.DT, drivers);
        apply_step(params.method, n, x.data(), y.data(), z.data(),
                   vx.data(), vy.data(), vz.data(),
                   atoms.masses.data(), params.G, params.B,
                   bonds, atoms.fixed.data(),
                   atoms.pos_0.data(),
                   params.box_a, params.DT,
                   params.gravity_field, params.gravity_interaction,
                   params.elastic_force, params.damping_force, params.gravity_strength);
    }

    // 记录
    int _prog_int = record_steps / 100;
    if (_prog_int < 1) _prog_int = 1;
    int si = 0;  // 采样帧索引
    for (int s = 0; s < record_steps; s++) {
        if (s % _prog_int == 0 && s > 0) {
            std::cout << "[Cpp-engine] progress: " << s << "/" << record_steps << std::endl;
        }
        double t = (s + params.warmup_steps) * params.DT;
        if (params.driving_force)
            apply_driving_force(n, x.data(), y.data(), z.data(), vx.data(), vy.data(), vz.data(), t, s, params.DT, drivers);
        // 保存当前帧（采样模式仅每 NSTEP 步保存一次）
        if (s % nstep_sampling == 0) {
            for (int i = 0; i < n; i++) {
                traj_x[si * n + i]  = x[i];
                traj_y[si * n + i]  = y[i];
                traj_z[si * n + i]  = z[i];
                traj_vx[si * n + i] = vx[i];
                traj_vy[si * n + i] = vy[i];
                traj_vz[si * n + i] = vz[i];
            }
            si++;
        }

        apply_step(params.method, n, x.data(), y.data(), z.data(),
                   vx.data(), vy.data(), vz.data(),
                   atoms.masses.data(), params.G, params.B,
                   bonds, atoms.fixed.data(),
                   atoms.pos_0.data(),
                   params.box_a, params.DT,
                   params.gravity_field, params.gravity_interaction,
                   params.elastic_force, params.damping_force, params.gravity_strength);
    }

    // 输出轨迹
    if (params.save_trajectory == 0) {
        std::string out_path = output_dir + "/display.txt";
        write_display_txt(out_path, traj_x, traj_y, traj_z, traj_vx, traj_vy, traj_vz,
                          si, n, params, atoms);
    } else {
        std::string out_path = output_dir + "/trajectory.txt";
        write_trajectory_json(out_path, traj_x, traj_y, traj_z, traj_vx, traj_vy, traj_vz,
                              record_steps, n, params, atoms, bonds);
    }

    auto t1 = std::chrono::high_resolution_clock::now();
    double elapsed = std::chrono::duration<double>(t1 - t0).count();
    std::cout << "[C++-engine] 计算完成: " << record_steps << " 步, " << elapsed << " s" << std::endl;

    return 0;
}