
#define DYNAMIC_LOAD
#include "ethercat.h"

#include <stdio.h>
#include <string.h>

#ifdef _WIN32
#include <windows.h>
#define SLEEP_MS(ms) Sleep(ms)
#else
#include <unistd.h>
#define SLEEP_MS(ms) usleep((ms) * 1000)
#endif

static void on_log(int category, const char* message)
{
    const char* tags[] = {"错误", "警告", "消息", "邮箱", "PDO", "调试"};
    const char* tag = (category >= 0 && category <= 5) ? tags[category] : "未知";
    printf("[%s] %s\n", tag, message);
}

static const char* cia402_state_name(cia402_state_t state)
{
    switch (state) {
    case CIA402_NOT_READY:          return "未就绪";
    case CIA402_SWITCH_ON_DISABLED: return "开关禁用";
    case CIA402_READY_TO_SWITCH_ON: return "准备开关";
    case CIA402_SWITCHED_ON:        return "已开关";
    case CIA402_OPERATION_ENABLED:  return "运行使能";
    case CIA402_QUICK_STOP_ACTIVE:  return "快速停止";
    case CIA402_FAULT_REACTION:     return "故障反应中";
    case CIA402_FAULT:              return "故障";
    default:                        return "未知";
    }
}

static const char* cia402_read_status_name(cia402_read_status_t st)
{
    switch (st) {
    case CIA402_READ_NOT_ATTEMPTED: return "未读取";
    case CIA402_READ_SUCCESS:       return "读取成功";
    case CIA402_READ_NOT_SUPPORTED: return "从站不支持";
    case CIA402_READ_FAILED:        return "读取失败";
    default:                        return "未知";
    }
}

static void cia402_read_observable_demo(dll_t* dll, uint16_t master, uint16_t slave,
                                        const char* name, uint16_t index, uint8_t sub)
{
    cia402_read_result_t r;
    r.status = CIA402_READ_NOT_ATTEMPTED;
    r.abort_code = 0; r.value64 = 0; r.byte_len = 0;

    if (dll->dx_sdo_read_ex) {
        int sz = 0; uint32_t abort = 0;
        char* p = dll->dx_sdo_read_ex(master, slave, index, sub, FALSE, &sz, &abort);
        r.abort_code = abort;
        if (p && sz > 0) {
            int n = sz < 8 ? sz : 8;
            uint8_t buf[8] = {0};
            memcpy(buf, p, (size_t)n);
            memcpy(&r.value64, buf, sizeof(r.value64));
            r.byte_len = sz;
            r.status = CIA402_READ_SUCCESS;
            if (dll->FreeMemory) dll->FreeMemory(p);
        } else {
            if (p && dll->FreeMemory) dll->FreeMemory(p);
            if (abort == EC_SDO_OBJECT_DOES_NOT_EXIST ||
                abort == EC_SDO_SUBINDEX_DOES_NOT_EXIST)
                r.status = CIA402_READ_NOT_SUPPORTED;
            else
                r.status = CIA402_READ_FAILED;
        }
    } else {
        r.status = CIA402_READ_FAILED;
    }

    if (r.status == CIA402_READ_SUCCESS)
        printf("  %s (0x%04X:%02X) = 0x%llX (%d 字节)\n",
               name, index, sub, (unsigned long long)r.value64, r.byte_len);
    else
        printf("  %s (0x%04X:%02X) = <%s, abort=0x%08X>\n",
               name, index, sub, cia402_read_status_name(r.status), r.abort_code);
}

static const char* cia402_mode_name(int8_t mode)
{
    switch (mode) {
    case CIA402_PP:  return "PP (轮廓位置)";
    case CIA402_VL:  return "VL (速度)";
    case CIA402_PV:  return "PV (轮廓速度)";
    case CIA402_PT:  return "PT (轮廓转矩)";
    case CIA402_HM:  return "HM (回零)";
    case CIA402_IP:  return "IP (插补位置)";
    case CIA402_CSP: return "CSP (周期同步位置)";
    case CIA402_CSV: return "CSV (周期同步速度)";
    case CIA402_CST: return "CST (周期同步转矩)";
    default:         return "未知";
    }
}

int main(int argc, char* argv[])
{
    dll_t dll;
    uint16_t master;
    uint16_t slave_index;
    const char* adapter;

    printf("=== DarraEtherCAT C SDK - CiA 402 伺服驱动示例 ===\n\n");

    if (argc < 3) {
        printf("用法: %s <网卡名称> <从站索引> [冗余网卡名称]\n", argv[0]);
        printf("  网卡名称: Windows 下为 \\\\Device\\\\NPF_{GUID}\n");
        printf("  从站索引: CiA 402 伺服驱动器的从站编号 (从1开始)\n");
        return 1;
    }

    adapter = argv[1];
    slave_index = (uint16_t)atoi(argv[2]);

    if (!LOAD_DLL(&dll, "Darra.Core.dll")) {
        printf("错误: 无法加载 Darra.Core.dll\n");
        return 1;
    }
    printf("DLL 加载成功\n");

    if (dll.SetLogCallback) {
        dll.SetLogCallback(on_log);
    }

    if (!dll.CiA402_Enable || !dll.CiA402_ReadStatusWord || !dll.CiA402_SetMode) {
        printf("错误: DLL 版本过低, 不支持 CiA 402 API\n");
        UNLOAD_DLL(&dll);
        return 1;
    }

    master = dll.Initialize();
    if (master == 0) {
        printf("错误: 主站初始化失败\n");
        UNLOAD_DLL(&dll);
        return 1;
    }
    printf("主站初始化成功, 索引: %d\n", master);

    {
        const char* secondary = (argc >= 4) ? argv[3] : "";
        uint16_t ret = dll.SetNetwork(master, adapter, secondary);
        if (ret == 0 || ret == 0xFFFF) {
            printf("错误: 网络绑定失败 (返回值: %u)\n", ret);
            dll.Dispose(master);
            UNLOAD_DLL(&dll);
            return 1;
        }
        printf("网络绑定成功, 从站数量: %d\n", ret);
    }

    printf("开始状态转换 (Init -> PreOp -> SafeOp -> OP)...\n");
    if (!dll.SetStateSequence(master, EC_STATE_OPERATIONAL, 15000)) {
        printf("错误: 状态转换失败\n");
        dll.Dispose(master);
        UNLOAD_DLL(&dll);
        return 1;
    }
    printf("已进入 Operational 状态\n");

    dll.Start(master);
    printf("PDO 循环已启动\n");
    SLEEP_MS(100);

    {
        uint16_t sw = dll.CiA402_ReadStatusWord(master, slave_index);
        cia402_state_t state = dll.CiA402_ParseState(sw);
        printf("从站 %d 初始状态: %s (StatusWord=0x%04X)\n",
               slave_index, cia402_state_name(state), sw);

        if (state == CIA402_FAULT) {
            printf("检测到故障, 正在复位...\n");
            if (dll.CiA402_FaultReset(master, slave_index) == 0) {
                printf("故障复位成功\n");
            } else {
                printf("错误: 故障复位失败\n");
                goto cleanup;
            }
            SLEEP_MS(200);
        }
    }

    printf("\n只读诊断对象 (可观测三态):\n");
    cia402_read_observable_demo(&dll, master, slave_index, "错误代码",     0x603F, 0);
    cia402_read_observable_demo(&dll, master, slave_index, "支持的驱动模式", 0x6502, 0);
    cia402_read_observable_demo(&dll, master, slave_index, "数字输入",     0x60FD, 0);
    cia402_read_observable_demo(&dll, master, slave_index, "操作模式显示",   0x6061, 0);
    cia402_read_observable_demo(&dll, master, slave_index, "厂商驱动数据",   0x6510, 1);

    printf("设置运行模式为 CSP (周期同步位置)...\n");
    if (dll.CiA402_SetMode(master, slave_index, CIA402_CSP) != 0) {
        printf("错误: 设置运行模式失败\n");
        goto cleanup;
    }
    SLEEP_MS(50);

    {
        int8_t mode = dll.CiA402_GetMode(master, slave_index);
        printf("当前运行模式: %s (0x%02X)\n", cia402_mode_name(mode), (uint8_t)mode);
        if (mode != CIA402_CSP) {
            printf("警告: 运行模式未成功切换\n");
        }
    }

    printf("正在使能驱动 (最多重试 50 次)...\n");
    if (dll.CiA402_Enable(master, slave_index, 50) != 0) {
        printf("错误: 驱动使能失败\n");

        uint16_t sw = dll.CiA402_ReadStatusWord(master, slave_index);
        cia402_state_t state = dll.CiA402_ParseState(sw);
        printf("当前状态: %s (StatusWord=0x%04X)\n",
               cia402_state_name(state), sw);
        goto cleanup;
    }
    printf("驱动使能成功!\n");

    printf("\n开始位置控制示例 (5 次循环)...\n");
    printf("注意: 偏移量需根据实际 PDO 映射配置调整\n\n");

    {
        const uint32_t OFFSET_TARGET_POS  = 2;
        const uint32_t OFFSET_ACTUAL_POS  = 2;
        int32_t target_position = 0;
        int i;

        for (i = 0; i < 5; i++) {

            int32_t actual_pos = dll.PdoReadInt32(master, slave_index, OFFSET_ACTUAL_POS);

            target_position = actual_pos + 1000;
            dll.PdoWriteInt32(master, slave_index, OFFSET_TARGET_POS, target_position);

            printf("[%d] 目标位置: %d, 实际位置: %d\n", i + 1, target_position, actual_pos);

            {
                uint16_t sw = dll.CiA402_ReadStatusWord(master, slave_index);
                cia402_state_t state = dll.CiA402_ParseState(sw);

                if (state == CIA402_FAULT || state == CIA402_FAULT_REACTION) {
                    printf("错误: 驱动器进入故障状态!\n");

                    if (dll.EmcyGetCount) {
                        int emcy_count = dll.EmcyGetCount(master, slave_index);
                        if (emcy_count > 0) {
                            ec_emcy_record_t records[8];
                            int count = dll.EmcyGetHistory(master, slave_index, records, 8);
                            printf("EMCY 历史 (%d 条):\n", count);
                            for (int j = 0; j < count; j++) {
                                printf("  [%d] 错误码=0x%04X 寄存器=0x%02X 时间=%ums\n",
                                       j, records[j].error_code, records[j].error_register,
                                       records[j].timestamp_ms);
                            }
                        }
                    }

                    printf("尝试故障复位...\n");
                    if (dll.CiA402_FaultReset(master, slave_index) == 0) {
                        printf("故障复位成功, 重新使能...\n");
                        SLEEP_MS(200);
                        if (dll.CiA402_Enable(master, slave_index, 20) == 0) {
                            printf("重新使能成功\n");
                            continue;
                        }
                    }
                    printf("恢复失败, 退出控制循环\n");
                    break;
                }
            }

            SLEEP_MS(500);
        }
    }

    printf("\n位置控制示例完成\n");

cleanup:

    printf("\n按 Enter 键停止...\n");
    getchar();

    printf("正在停止...\n");
    dll.Stop(master);
    dll.ClearStartupParameters(master, 0);
    dll.Dispose(master);
    UNLOAD_DLL(&dll);
    printf("已退出\n");

    return 0;
}
