/*
 * 程序功能为通过EtherCAT周期控制伺服电机
 *
 * 功能总览：
 * 1) 初始化 EtherCAT Master / Domain / 从站配置（PDO/同步管理/DC 时钟）；
 * 2) 注册 PDO 映射，建立 process image 的偏移（offsets[]）；
 * 3) 启动 1kHz 实时循环（cyclic_task）：
 *    - 同步 DC 时钟，收发域数据；
 *    - 跟踪各轴 DS402 状态机（Ready->Switched on->Operation enabled）；
 *    - 根据上层 g_cmd（CSV/CSP/CST/PVT）写入目标值到 PDO；

*/
#include <errno.h>
#include <signal.h>
#include <stdio.h>
#include <string.h>
#include <sys/resource.h>
#include <sys/time.h>
#include <sys/types.h>
#include <unistd.h>
#include <time.h>
#include <sys/mman.h>
#include <malloc.h>
#include <sched.h> /* sched_setscheduler() */
#include <thread>
#include <atomic>
#include <pthread.h>
#include <cmath>   // for std::llround
#include <iostream>



#include "ethercat_control/mt_config.hpp"
#include "ethercat_control/zer_config.hpp"
#include "ethercat_control/et_config.hpp"
// #include "ethercat_control/ds_config.hpp"


//#include "cmd_interface.hpp" //命令行控制库


// sudo /opt/etherlab/bin/ethercat
// sudo /etc/init.d/ethercat start
// sudo dmesg | grep EtherCAT
// sudo /opt/etherlab/bin/ethercat cstruct -a 0

// ------------------- 应用参数 -------------------
#define FREQUENCY 1000                          //控制周期频率1000Hz
#define CLOCK_TO_USE CLOCK_MONOTONIC

//#define MEASURE_TIMING

//#define NUM_SLAVES                 3          //从端数量在motor_control.hpp中定义
//#define TARGET_VALUE             10000        //目标位置、速度或者扭矩

#define CYCLIC_POSITION            8            //CSP 周期同步位置模式
#define CYCLIC_VELOCITY            9            //CSP 周期同步速度模式
#define CYCLIC_TORQUE              10           //CSP 周期同步扭矩模式
#define PVT_MODE                   5            //PVT模式

#define CSP_MAX_VEL_COUNTS_PER_S    65536 //65536        //CSP最大速度（计数/s）
//#define CSP_MAX_VEL_COUNTS_PER_S   1             //低速测试CSP最大速度（计数）

#define CSP_POS_DEADBAND           10           //CSP允许的误差(计数)

#define NSEC_PER_SEC (1000000000L)
#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)    //本次设置周期PERIOD_NS为1ms

#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
        (B).tv_nsec - (A).tv_nsec)

#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)

static std::atomic<bool> g_started{false}; //g_started 是一个进程内的“已启动”标志
static std::atomic<bool> g_run{true}; //g_run是运行标志，为1时电机可运行


// ------------------- EtherCAT 句柄 -------------------
static ec_master_t *master = NULL;
static ec_master_state_t master_state = {};

static ec_domain_t *domain1 = NULL;
static ec_domain_state_t domain1_state = {};

static ec_slave_config_t *sc[NUM_SLAVES] = {0}; 
static ec_slave_config_state_t sc_state[NUM_SLAVES] = {};

static uint8_t *domain1_pd = NULL; // process data

static unsigned int counter = 0;
static unsigned int sync_ref_counter = 0;
const struct timespec cycletime = {0, PERIOD_NS};

// ------------------- 工具函数 -------------------
// 相加时间并处理纳秒进位
struct timespec timespec_add(struct timespec time1, struct timespec time2)
{
    struct timespec result;

    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
    } else {
        result.tv_sec = time1.tv_sec + time2.tv_sec;
        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
    }

    return result;
}

//查看domain1工作状态
void check_domain1_state(void)
{
    ec_domain_state_t ds;

    ecrt_domain_state(domain1, &ds);

    if (ds.working_counter != domain1_state.working_counter)
        //printf("Domain1: WC %u.\n", ds.working_counter); //当数字变化，表示丢包
    if (ds.wc_state != domain1_state.wc_state)
        //printf("Domain1: State %u.\n", ds.wc_state);

    domain1_state = ds;
}

//查看master工作状态
void check_master_state(void)
{
    ec_master_state_t ms;

    ecrt_master_state(master, &ms);

    if (ms.slaves_responding != master_state.slaves_responding)
        //printf("%u slave(s).\n", ms.slaves_responding);
    if (ms.al_states != master_state.al_states)
        //printf("AL states: 0x%02X.\n", ms.al_states);
    if (ms.link_up != master_state.link_up)
        //printf("Link is %s.\n", ms.link_up ? "up" : "down");

    master_state = ms;
}

//查询并更新每个从站的在线/AL/operational 状态
void check_slave_config_states(void)
{
    ec_slave_config_state_t s;
    for (int i = 0; i < NUM_SLAVES; ++i) {
        if (!sc[i]) continue;
        ecrt_slave_config_state(sc[i], &s);
        if (s.al_state != sc_state[i].al_state)
            //printf("[S%02d] State 0x%02X.\n", i, s.al_state);
        if (s.online != sc_state[i].online)
            //printf("[S%02d] %s.\n", i, s.online ? "online" : "offline");
        if (s.operational != sc_state[i].operational)
            //printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
        sc_state[i] = s;
    }
}

// ------------------- 实时循环 -------------------
/**
 *  1kHz 实时控制主循环
 *
 * 核心流程（每周期）：
 *  1) 绝对时钟休眠到下个周期；写入应用时间到主站；
 *  2) 接收帧 / 处理域数据；
 *  3) 遍历每个从站：
 *     - 读取 TxPDO：状态字/实际位置/速度/扭矩/模式显示；
 *     - 把读到的实际值写回共享状态 g_state（给上位机/ROS2）；
 *     - 读取上层命令 g_cmd：期望模式、run_enable、pos/vel/tor、PVT kp/kd；
 *     - 模式切换：降级到perop→写 0x6060→初始化必要变量→进入下周期再跑；
 *     - 跟踪 DS402 状态机，达到 Operation enabled 后根据模式写入：
 *         CSP：写 0x607A；
 *         CSV：写 0x60FF；
 *         CST：写 0x6071；
 *         PVT：写 0x607A/0x60FF/0x6071 及 KP/KD(0x2000/0x2001)；
 *     - run_enable=false：安全处理（速度清零、CSP 对齐当前位置、扭矩清零）。
 *  4) 同步 DC 参考时钟与从站时钟；队列域数据并发送帧。
 *
 */

int lastStatus = 0;
int motorCount1 = 0;
int motorCount2 = 0;

void cyclic_task()
{
    struct timespec wakeupTime, time;
#ifdef MEASURE_TIMING
    struct timespec startTime, endTime, lastStartTime = {};
    uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0,
             latency_min_ns = 0, latency_max_ns = 0,
             period_min_ns = 0, period_max_ns = 0,
             exec_min_ns = 0, exec_max_ns = 0;
#endif

    clock_gettime(CLOCK_TO_USE, &wakeupTime);        //当前时间
    static uint16_t command[NUM_SLAVES];             //状态字掩码
    static uint16_t status[NUM_SLAVES];              //状态字
    static uint16_t last_status[NUM_SLAVES];         //上个循环的状态字
    static int inited = 0;                           //初始化

    while(g_run.load(std::memory_order_relaxed)) {   //主循环
        wakeupTime = timespec_add(wakeupTime, cycletime);
        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);    //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移

        // Write application time to master
        //
        // It is a good idea to use the target time (not the measured time) as
        // application time, because it is more stable.
        //
        ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));  // 更新主站应用时间

#ifdef MEASURE_TIMING
        clock_gettime(CLOCK_TO_USE, &startTime);
        latency_ns = DIFF_NS(wakeupTime, startTime);
        period_ns = DIFF_NS(lastStartTime, startTime);
        exec_ns = DIFF_NS(lastStartTime, endTime);
        lastStartTime = startTime;

        if (latency_ns > latency_max_ns) {
            latency_max_ns = latency_ns;
        }
        if (latency_ns < latency_min_ns) {
            latency_min_ns = latency_ns;
        }
        if (period_ns > period_max_ns) {
            period_max_ns = period_ns;
        }
        if (period_ns < period_min_ns) {
            period_min_ns = period_ns;
        }
        if (exec_ns > exec_max_ns) {
            exec_max_ns = exec_ns;
        }
        if (exec_ns < exec_min_ns) {
            exec_min_ns = exec_ns;
        }
#endif
        // receive process data
        ecrt_master_receive(master);    //接收 EtherCAT 帧
        ecrt_domain_process(domain1);   //处理域数据

        // check process data state (optional)
        check_domain1_state();      //检查域状态

        if (counter) {
            counter--;
        } else { // do this at 1 Hz
            counter = FREQUENCY;

        // check for master state (optional)
        check_master_state();               //检查主站状态
		// check for slave configuration state(s)
		check_slave_config_states();        //检查从站状态

#ifdef MEASURE_TIMING
            // output timing stats
            printf("period     %10u ... %10u\n",
                    period_min_ns, period_max_ns);
            printf("exec       %10u ... %10u\n",
                    exec_min_ns, exec_max_ns);
            printf("latency    %10u ... %10u\n",
                    latency_min_ns, latency_max_ns);
            printf("status word: 0x%4x\n",status);
            period_max_ns = 0;
            period_min_ns = 0xffffffff;
            exec_max_ns = 0;
            exec_min_ns = 0xffffffff;
            latency_max_ns = 0;
            latency_min_ns = 0xffffffff;
#endif

        }
        
        if (!inited) {
            for (int i = 0; i < NUM_SLAVES; ++i) {
                command[i] = 0x004F;
                status[i] = 0x000F;
                last_status[i] = status[i];
            }
            inited = 1;
        }

        static uint8_t csp_initialized[NUM_SLAVES] = {0};                    // 进入CSP后是否已对齐过目标（防止冲击）
        static uint8_t last_mode_cmd_inited = 0;  
        static int8_t  last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY};        // 记录上周期的期望模式（默认为CSV）  

        // 读取从站状态字、当前位置、速度、扭矩、运行模式
        for (int i = 0; i < NUM_SLAVES; ++i) 
        {                          
            if (i == 0)
            // if (false)
            {
                uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
                int32_t  pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
                int32_t  vv = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
                int16_t  tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
                int8_t   md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);

                status[i] = sw;                                 //侦测子站状态字变化
                if (status[i] != last_status[i]) {
                    // printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
                    last_status[i] = status[i];
                }
                
                // ----- 1) 写共享状态（供ROS2 读取）-----
                g_state.status_word[i].store(sw, std::memory_order_relaxed);
                g_state.pos_act[i].store(pv,    std::memory_order_relaxed);
                g_state.vel_act[i].store(vv,    std::memory_order_relaxed);
                g_state.torque_act[i].store(tv, std::memory_order_relaxed);

                // ----- 2) 读取本周期命令 -----
                // 读CSP/CSV/CST/PVT 命令
                const int8_t  mode_cmd   = g_cmd.mode[i].load(std::memory_order_relaxed);
                const bool    run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
                const int32_t vel_cmd    = g_cmd.target_vel[i].load(std::memory_order_relaxed);
                const int32_t pos_cmd    = g_cmd.target_pos[i].load(std::memory_order_relaxed);
                const int16_t tor_cmd    = g_cmd.target_torque[i].load(std::memory_order_relaxed);

                //EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定

                // 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
                if (!last_mode_cmd_inited) {
                    last_mode_cmd[i] = md;
                    if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
                }

                if (md != mode_cmd) {
                    // 1) 拉到 Ready to switch on（允许改模式）
                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
                    // 2) 写目标模式
                    EC_WRITE_S8 (domain1_pd + zer_offsets[i].operation_mode, mode_cmd); 
                    // 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
                    // 进入新模式时做必要初始化

                    bool csp_mode = (mode_cmd == CYCLIC_POSITION);
                    if (csp_mode) {
                        // 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
                        // g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
                        // EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv);
                        
                        csp_initialized[i] = 1;
                    } else {
                        csp_initialized[i] = 0;
                    }
                    last_mode_cmd[i] = mode_cmd;
                    continue; // 本周期先不再下其他目标，等模式生效
                }
                
                // DS402 状态机 & 写控制/目标
                if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);   // Ready to switch on
                    command[i] = 0x006F;
                    std::cout << " Motor : " << i << "  Ready to switch on" << std::endl;
                } else if ((status[i] & command[i]) == 0x0021) {
                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);   // Switched on
                    command[i] = 0x006F;

                    // 同步当前位置为目标位置
                    int32_t  pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
                    EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
                    g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);

                    motorCount1 ++;
                    std::cout << " Motor : " << i  << " times : " << motorCount1 << " Switched on" << std::endl;
                } else if ((status[i] & command[i]) == 0x0023) {
                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f);   // Operation enabled
                    command[i] = 0x006F;

                    std::cout << " Motor : " << i << "  Operation enabled" << std::endl;
                } else if ((status[i] & command[i]) == 0x0027) {      
                    // ---- 4) 运行态写目标（按模式/运行意图） ----
                    if (run_enable) {
                        //EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
                        switch (mode_cmd) {
                            case CYCLIC_VELOCITY: // 9
                                EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
                                break;
                            case CYCLIC_POSITION: { // 8
                                // ---- 绝对式目标生成，不再用 pv 做增量基准 ----
                                // 每轴记住“上次下发的命令位置”
                                static int32_t csp_cmd_pos[NUM_SLAVES];         // 上次下发到 0x607A 的值
                                static uint8_t csp_inited[NUM_SLAVES] = {0};    // 是否已初始化
                                // 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
                                static int64_t vmax_acc[NUM_SLAVES] = {0};      // 单位：counts/s 累加器

                                // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
                                if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
                                    csp_cmd_pos[i] = pv;       // 先对齐
                                    vmax_acc[i]     = 0;
                                    csp_inited[i]   = 1;
                                }

                                // 期望的“最终绝对目标”
                                const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);

                                // 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
                                vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S;     // +v [counts/s]
                                int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
                                vmax_acc[i] -= (int64_t)max_step * FREQUENCY;          // 保留余数到下周期

                                // 相对“命令轨迹”的误差（不是相对 pv）
                                const int32_t err_cmd = goal - csp_cmd_pos[i];

                                // 死区：足够近就贴合到 goal
                                if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
                                    csp_cmd_pos[i] = goal;
                                } else {
                                    // 限速迈步：命令轨迹只按时间往 goal 逼近
                                    if (err_cmd > 0)      csp_cmd_pos[i] += (err_cmd >  max_step) ?  max_step : err_cmd;
                                    else                  csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
                                }

                                // 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
                                // const int32_t follow_err = csp_cmd_pos[i] - pv;
                                // const int32_t FE_LIMIT =  (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
                                // if (std::abs(follow_err) > FE_LIMIT) {
                                //     // 例如：冻结或减半 max_step，给驱动时间追上
                                // }

                                // 下发“绝对目标”（不是增量）
                                EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);

                                // 回显给上层（/joint_states_cmd 用）
                                g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
                                break;
                            }
                            case CYCLIC_TORQUE:   // 10
                                EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
                                break;
                            default:
                                // 未知模式：默认速度模式安全置 0
                                //EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
                                break;
                                
                        }
                    } else {
                        // run_enable=false：清零目标
                        EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
                        // CSV 安全：速度清零
                        EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
                        // CSP 安全：目标位置=当前位置 -> 立即“冻结”
                        
                        int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
                        EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
                        // 可选：扭矩模式也清零（最好维持急停状态，待修改）
                        EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
                        EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
                    }
                }
            }else{
                uint16_t sw = EC_READ_U16(domain1_pd + mt_offsets[i].status_word);
                int32_t  pv = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
                int32_t  vv = EC_READ_S32(domain1_pd + mt_offsets[i].velocity_actual_value);
                int16_t  tv = EC_READ_S16(domain1_pd + mt_offsets[i].torque_actual_value);
                int8_t   md = EC_READ_S8 (domain1_pd + mt_offsets[i].modes_of_operation_display);

                // printf("[S%02d] 状态字: 0x%04X, 位置: %d, 速度: %d, 力矩: %d, 模式: %d\n", i, sw, pv, vv, tv, md);

                status[i] = sw;                                 //侦测子站状态字变化
                if (status[i] != last_status[i]) {
                    // printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
                    last_status[i] = status[i];
                }
                
                // ----- 1) 写共享状态（供ROS2 读取）-----
                g_state.status_word[i].store(sw, std::memory_order_relaxed);
                g_state.pos_act[i].store(pv,    std::memory_order_relaxed);
                g_state.vel_act[i].store(vv,    std::memory_order_relaxed);
                g_state.torque_act[i].store(tv, std::memory_order_relaxed);

                // ----- 2) 读取本周期命令 -----
                // 读CSP/CSV/CST/PVT 命令
                const int8_t  mode_cmd   = g_cmd.mode[i].load(std::memory_order_relaxed);
                const bool    run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
                const int32_t vel_cmd    = g_cmd.target_vel[i].load(std::memory_order_relaxed);
                const int32_t pos_cmd    = g_cmd.target_pos[i].load(std::memory_order_relaxed);
                const int16_t tor_cmd    = g_cmd.target_torque[i].load(std::memory_order_relaxed);

                //EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定

                // 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
                if (!last_mode_cmd_inited) {
                    last_mode_cmd[i] = md;
                    if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
                }

                if (md != mode_cmd) {
                    // 1) 拉到 Ready to switch on（允许改模式）
                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006);
                    // 2) 写目标模式
                    EC_WRITE_S8 (domain1_pd + mt_offsets[i].operation_mode, mode_cmd); 
                    // 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
                    // 进入新模式时做必要初始化
                    if (mode_cmd == CYCLIC_POSITION) {
                        // 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
                        // g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
                        // EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv);
                        csp_initialized[i] = 1;

                        // std::cout << "new Goal : " << pv << std::endl;

                    } else {
                        csp_initialized[i] = 0;
                    }
                    last_mode_cmd[i] = mode_cmd;
                    continue; // 本周期先不再下其他目标，等模式生效
                }
                
                // DS402 状态机 & 写控制/目标
                if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006);   // Ready to switch on
                    command[i] = 0x006F;
                    std::cout << " Motor : " << i << "  Ready to switch on" << std::endl;
                } else if ((status[i] & command[i]) == 0x0021) {
                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);   // Switched on
                    command[i] = 0x006F;

                    // 同步当前位置为目标位置
                    int32_t  pv2 = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
                    EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv2);
                    g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);

                    motorCount2 ++;
                    std::cout << " Motor : " << i << " times : " << motorCount2 << "  Switched on" << std::endl;
                } else if ((status[i] & command[i]) == 0x0023) {
                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x000f);   // Operation enabled
                    command[i] = 0x006F;
                    std::cout << " Motor : " << i << "  Operation enabled" << std::endl;
                } else if ((status[i] & command[i]) == 0x0027) {
                    // ---- 4) 运行态写目标（按模式/运行意图） ----
                    if (run_enable) {
                        switch (mode_cmd) {
                            case CYCLIC_VELOCITY: // 9
                                EC_WRITE_S32(domain1_pd + mt_offsets[i].target_velocity, vel_cmd);
                                break;
                            case CYCLIC_POSITION: { // 8
                                // ---- 绝对式目标生成，不再用 pv 做增量基准 ----
                                // 每轴记住“上次下发的命令位置”
                                static int32_t csp_cmd_pos[NUM_SLAVES];         // 上次下发到 0x607A 的值
                                static uint8_t csp_inited[NUM_SLAVES] = {0};    // 是否已初始化
                                // 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
                                static int64_t vmax_acc[NUM_SLAVES] = {0};      // 单位：counts/s 累加器

                                // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
                                if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
                                    csp_cmd_pos[i] = pv;       // 先对齐
                                    vmax_acc[i]     = 0;
                                    csp_inited[i]   = 1;
                                }

                                // 期望的“最终绝对目标”
                                const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);

                                // std::cout << "goal: " << goal << std::endl;

                                // 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
                                vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S;     // +v [counts/s]
                                int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
                                vmax_acc[i] -= (int64_t)max_step * FREQUENCY;          // 保留余数到下周期

                                // 相对“命令轨迹”的误差（不是相对 pv）
                                const int32_t err_cmd = goal - csp_cmd_pos[i];

                                // 死区：足够近就贴合到 goal
                                if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
                                    csp_cmd_pos[i] = goal;
                                } else {
                                    // 限速迈步：命令轨迹只按时间往 goal 逼近
                                    if (err_cmd > 0)      csp_cmd_pos[i] += (err_cmd >  max_step) ?  max_step : err_cmd;
                                    else                  csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
                                }

                                // 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
                                // const int32_t follow_err = csp_cmd_pos[i] - pv;
                                // const int32_t FE_LIMIT =  (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
                                // if (std::abs(follow_err) > FE_LIMIT) {
                                //     // 例如：冻结或减半 max_step，给驱动时间追上
                                // }

                                // 下发“绝对目标”（不是增量）
                                EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, csp_cmd_pos[i]);

                                // 回显给上层（/joint_states_cmd 用）
                                g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
                                break;
                            }
                            case CYCLIC_TORQUE:   // 10
                                EC_WRITE_S16(domain1_pd + mt_offsets[i].target_torque, tor_cmd);
                                break;
                            default:
                                // 未知模式：默认速度模式安全置 0
                                //EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
                                break;
                                
                        }
                    } else {
                        // run_enable=false：清零目标
                        EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);
                        // CSV 安全：速度清零
                        EC_WRITE_S32(domain1_pd + mt_offsets[i].target_velocity, 0);
                        // CSP 安全：目标位置=当前位置 -> 立即“冻结”
                        int32_t pv_hold = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
                        EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv_hold);
                        // 可选：扭矩模式也清零（最好维持急停状态，待修改）
                        EC_WRITE_S16(domain1_pd + mt_offsets[i].target_torque, 0);
                        EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
                    }
                }
            }
        }
        
        if (sync_ref_counter) {
            sync_ref_counter--;
        } else {
            sync_ref_counter = 1; // sync every 2 cycle

            clock_gettime(CLOCK_TO_USE, &time);
            ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
        }
        ecrt_master_sync_slave_clocks(master);

        // send process data
        ecrt_domain_queue(domain1);     //排队域数据
        ecrt_master_send(master);   //发送ETHERCAT帧

#ifdef MEASURE_TIMING
        clock_gettime(CLOCK_TO_USE, &endTime);
#endif
    }
}
// ------------------- 启动/停机接口 -------------------
namespace ethercat_core {

//启动实时线程
static void start_rt_cyclic_thread(){   
  std::thread([]{
    // 锁内存 & 实时优先级
    mlockall(MCL_CURRENT | MCL_FUTURE); //将当前进程使用的所有内存（MCL_CURRENT）以及将来可能分配的所有内存（MCL_FUTURE）都锁定在物理 RAM 中，禁止将其交换到磁盘（Swap）
    sched_param param{}; param.sched_priority = sched_get_priority_max(SCHED_FIFO); //将当前线程的调度策略设置为 SCHED_FIFO (First-In, First-Out)，并赋予最高优先级
    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
    cyclic_task(); // 进入控制循环（阻塞）
  }).detach();
}

/**
 * 初始化 EtherCAT master / domain / PDO / DC 并启动实时循环
 * return true 成功；false 失败（会复位 g_started 标志）
 *
 * 步骤：
 *  1) ecrt_request_master / ecrt_master_create_domain；
 *  2) 根据 use_pvt 选择配置 device_syncs 或 device_syncs_pvt；
 *  3) ecrt_slave_config_dc 启用 DC 同步；
 *  4) ecrt_domain_reg_pdo_entry_list 注册 PDO entry，保存 offsets[];
 *  5) ecrt_master_activate + ecrt_domain_data 获取 process image 指针；
 *  6) 设定上层默认模式（PVT 或 CSV），拉起 run_enable；
 *  7) 启动实时线程。
 */
bool start()
{
    printf("Starting EtherCAT master...\n");
    if (g_started.exchange(true)) {
        // 已经启动
        return true;
    }
    // 申请 master / 域
    master = ecrt_request_master(0);
    if (!master) { 
        perror("ecrt_request_master"); 
        g_started.store(false); return false; 
    }
    domain1 = ecrt_master_create_domain(master);
    if (!domain1){ 
        perror("ecrt_master_create_domain"); 
        g_started.store(false); return false; 
    }    

    // 从站配置
    for (int i = 0; i < NUM_SLAVES; ++i) {

        std::cout << "Configuring slave " << i << "..." << std::endl;
        
        int position = i+1;

        // if (i > 0)
        // {
        //     position = i+2;
        // }

        if (i == 0)
        {
            sc[i] = ecrt_master_slave_config(master, 0, position, ZER_VID_PID);
            if (!sc[i]) 
            { 
                std::cout << "Failed slave cfg at pos " << i << std::endl;
                // fprintf(stderr,"Failed slave cfg at pos %d\n", i); g_started.store(false); return false; 
            }

            if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs)) 
            {
                std::cout << "Failed PDO config " << i << std::endl;
                //   fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false;
            }
        }
        else{

            sc[i] = ecrt_master_slave_config(master, 0, position, MT_VID_PID);
            if (!sc[i]) 
            { 
                std::cout << "Failed slave cfg at pos " << i << std::endl;
            }

            if (ecrt_slave_config_pdos(sc[i], EC_END, mt_device_syncs)) 
            {
                std::cout << "Failed PDO config " << i << std::endl;
            }
        }

        // DC配置
        ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
    }

   
    // 注册 PDO entry 偏移
    for (int i = 0; i < NUM_SLAVES; ++i) {

        int position = i+1;

        // if (i > 1)
        // {
        //     position = i+2;
        // }

        if (i == 0)
        {
            // ---- CSP/CSV/CST PDO ----
            const size_t N = sizeof(zer_domain1_regs)/sizeof(zer_domain1_regs[0]);
            ec_pdo_entry_reg_t regs[N];
            memcpy(regs, zer_domain1_regs, sizeof(zer_domain1_regs));
            for (size_t j = 0; j + 1 < N; ++j) 
            { 
                regs[j].alias = 0; 
                regs[j].position = position; 
            }

            regs[0].offset  = &zer_offsets[i].ctrl_word;
            regs[1].offset  = &zer_offsets[i].target_position;
            regs[2].offset  = &zer_offsets[i].target_velocity;
            regs[3].offset  = &zer_offsets[i].target_torque;
            regs[4].offset  = &zer_offsets[i].max_torque;
            regs[5].offset  = &zer_offsets[i].operation_mode;
            regs[6].offset  = &zer_offsets[i].reserved1;
            regs[7].offset  = &zer_offsets[i].status_word;
            regs[8].offset  = &zer_offsets[i].position_actual_value;
            regs[9].offset  = &zer_offsets[i].velocity_actual_value;
            regs[10].offset = &zer_offsets[i].torque_actual_value;
            regs[11].offset = &zer_offsets[i].error_code;
            regs[12].offset = &zer_offsets[i].modes_of_operation_display;
            regs[13].offset = &zer_offsets[i].reserved2;

            if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
                fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
                g_started.store(false); return false;
            }
        }else{
            // ---- CSP/CSV/CST PDO ----
            const size_t N = sizeof(mt_domain1_regs)/sizeof(mt_domain1_regs[0]);
            ec_pdo_entry_reg_t regs[N];
            memcpy(regs, mt_domain1_regs, sizeof(mt_domain1_regs));
            for (size_t j = 0; j + 1 < N; ++j) 
            { 
                regs[j].alias = 0; 
                regs[j].position = position; 
            }

            regs[0].offset  = &mt_offsets[i].ctrl_word;
            regs[1].offset  = &mt_offsets[i].target_position;
            regs[2].offset  = &mt_offsets[i].target_velocity;
            regs[3].offset  = &mt_offsets[i].target_torque;
            regs[4].offset  = &mt_offsets[i].max_torque;
            regs[5].offset  = &mt_offsets[i].operation_mode;
            regs[6].offset  = &mt_offsets[i].reserved1;
            regs[7].offset  = &mt_offsets[i].status_word;
            regs[8].offset  = &mt_offsets[i].position_actual_value;
            regs[9].offset  = &mt_offsets[i].velocity_actual_value;
            regs[10].offset = &mt_offsets[i].torque_actual_value;
            regs[11].offset = &mt_offsets[i].error_code;
            regs[12].offset = &mt_offsets[i].modes_of_operation_display;
            regs[13].offset = &mt_offsets[i].reserved2;

            if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
                fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
                g_started.store(false); return false;
            }
        }
    }

    // 5) 激活 & 取 process image 指针
    if (ecrt_master_activate(master)) { perror("ecrt_master_activate"); g_started.store(false); return false; }
    domain1_pd = ecrt_domain_data(domain1);
    if (!domain1_pd) 
    { 
        fprintf(stderr,"domain1_pd null\n"); 
        g_started.store(false); 
        return false; 
    }

    std::cout << "Starting EtherCAT master done." << std::endl;

    mc_init_defaults(OpMode::CSP);

    mc_enable_all(true);
    // mc_set_all_target_velocity(0);

    // 启动实时循环线程
    start_rt_cyclic_thread();
    return true;
}

void stop_soft(){
    // 软停：拉低 run_enable；保持 S7 switched on，速度清零
    mc_enable_all(false);
}
void request_exit(){
    g_run.store(false);                  // 让循环跳出
}
}    // namespace ethercat_core