//============================================================================
// pmotorTest：pMotor 推进电机 CAN 协议编解码单元测试
// 无第三方测试库，独立可执行。参照 test/pccu/pccuTest.cpp 的轻量风格。
//
// 协议依据：docs/推进电机通信协议20260321.docx
//============================================================================

#include <cstdio>
#include <cstdlib>
#include <cstring>
#include <string>
#include <vector>

#include "../../src/pMotor/protocol/MotorCan.h"

using namespace motor;

static int g_fail = 0;
static int g_pass = 0;

#define CHECK(cond) do { \
    if (cond) { ++g_pass; } \
    else { ++g_fail; std::printf("FAIL %s:%d  %s\n", __FILE__, __LINE__, #cond); } \
} while (0)

#define CHECK_STREQ(a, b) do { \
    if (std::string(a) == std::string(b)) { ++g_pass; } \
    else { ++g_fail; std::printf("FAIL %s:%d  \"%s\" != \"%s\"\n", __FILE__, __LINE__, std::string(a).c_str(), std::string(b).c_str()); } \
} while (0)

//--------------------------------------------------------------------------
// 1. 指令帧编码（对照文档示例）

static void testCmdEncode() {
    uint8_t d[8];

    // 文档示例：启动并给定 1000 转速 -> 01 00 E8 03 00 00 00 00
    buildMotorCmdData(true, false, 1000, d);
    uint8_t expect1[] = {0x01, 0x00, 0xE8, 0x03, 0x00, 0x00, 0x00, 0x00};
    CHECK(std::memcmp(d, expect1, 8) == 0);

    // 文档示例：停机 -> 00 00 00 00 00 00 00 00
    buildMotorCmdData(false, false, 0, d);
    uint8_t expect0[] = {0, 0, 0, 0, 0, 0, 0, 0};
    CHECK(std::memcmp(d, expect0, 8) == 0);

    // 文档示例：复位 -> 02 00 00 00 00 00 00 00
    buildMotorCmdData(false, true, 0, d);
    uint8_t expect2[] = {0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    CHECK(std::memcmp(d, expect2, 8) == 0);

    // 使能+复位 同时有效 -> bit0+bit1 = 0x03
    buildMotorCmdData(true, true, 0, d);
    CHECK(d[0] == 0x03);

    // 负转速 s16 Intel：-100 = 0xFF9C -> byte2=0x9C byte3=0xFF
    buildMotorCmdData(true, false, -100, d);
    CHECK(d[0] == 0x01);
    CHECK(d[2] == 0x9C && d[3] == 0xFF);

    // 边界转速：32767 / -32768
    buildMotorCmdData(true, false, 32767, d);
    CHECK(d[2] == 0xFF && d[3] == 0x7F);
    buildMotorCmdData(true, false, -32768, d);
    CHECK(d[2] == 0x00 && d[3] == 0x80);

    // 支路通信状态位（bit2/bit3）恒 0（pMotor 不反馈通信状态）
    buildMotorCmdData(true, false, 1, d);
    CHECK((d[0] & 0x0C) == 0);
}

//--------------------------------------------------------------------------
// 2. CAN ID 识别

static void testCanId() {
    // 12 条电机 ID 全部命中
    const uint32_t ids[] = {
        MOTOR_CAN_CMD, MOTOR_CAN_CMD_RED,
        MOTOR_CAN_LIMIT, MOTOR_CAN_LIMIT_RED,
        MOTOR_CAN_FAULT1, MOTOR_CAN_FAULT1_RED,
        MOTOR_CAN_FAULT2, MOTOR_CAN_FAULT2_RED,
        MOTOR_CAN_STATE3, MOTOR_CAN_STATE3_RED,
        MOTOR_CAN_STATE4, MOTOR_CAN_STATE4_RED,
    };
    for (size_t i = 0; i < sizeof(ids) / sizeof(ids[0]); ++i)
        CHECK(isCanMotorId(ids[i]));

    // 冗余 CAN 与主 CAN 仅末字节差 1
    CHECK(MOTOR_CAN_CMD_RED == MOTOR_CAN_CMD + 1);
    CHECK(MOTOR_CAN_FAULT1_RED == MOTOR_CAN_FAULT1 + 1);
    CHECK(MOTOR_CAN_FAULT2_RED == MOTOR_CAN_FAULT2 + 1);
    CHECK(MOTOR_CAN_STATE3_RED == MOTOR_CAN_STATE3 + 1);
    CHECK(MOTOR_CAN_STATE4_RED == MOTOR_CAN_STATE4 + 1);

    // 其他协议报文不命中（BCU 电池 / 配电）
    CHECK(!isCanMotorId(0x10010000));   // BCU 概要
    CHECK(!isCanMotorId(0x103681FF));   // BCU 继电器控制
    CHECK(!isCanMotorId(0x1401A501));   // 配电控制指令
    CHECK(!isCanMotorId(0x18EF2012));   // 电机协议未定义的 ID
}

//--------------------------------------------------------------------------
// 3. 状态帧3 解码（0x18EF1220：反馈转速/母线电压）

static void testState3Decode() {
    MotorState st;
    uint8_t d[8] = {0xE8, 0x03, 0xF4, 0x01, 0, 0, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_STATE3, d, 8, st));
    CHECK(st.state3Valid);
    CHECK(st.feedbackSpeed == 1000);   // 0x03E8 Intel
    CHECK(st.busVoltage == 500);       // 0x01F4 Intel

    // 负转速
    uint8_t d2[8] = {0x9C, 0xFF, 0, 0, 0, 0, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_STATE3, d2, 8, st));
    CHECK(st.feedbackSpeed == -100);

    // 冗余 CAN 同样可解码
    MotorState st2;
    CHECK(decodeMotorFrame(MOTOR_CAN_STATE3_RED, d, 8, st2));
    CHECK(st2.feedbackSpeed == 1000 && st2.busVoltage == 500);

    // 非法 dlc
    MotorState st3;
    CHECK(!decodeMotorFrame(MOTOR_CAN_STATE3, d, 4, st3));
    CHECK(!decodeMotorFrame(MOTOR_CAN_STATE3, nullptr, 8, st3));
}

//--------------------------------------------------------------------------
// 4. 状态帧4 解码（0x18EF1320：温度）

static void testState4Decode() {
    MotorState st;
    uint8_t d[8] = {26, 44, 62, 79, 90, 107, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_STATE4, d, 8, st));
    CHECK(st.state4Valid);
    CHECK(st.tempA == 26 && st.tempB == 44 && st.tempC == 62);
    CHECK(st.tempInv == 79 && st.tempFilter == 90 && st.tempAir == 107);

    // 冗余 CAN
    MotorState st2;
    CHECK(decodeMotorFrame(MOTOR_CAN_STATE4_RED, d, 8, st2));
    CHECK(st2.tempA == 26);
}

//--------------------------------------------------------------------------
// 5. 故障帧解码（0x18EF1020 支路一 / 0x18EF1120 支路二）

static void testFaultDecode() {
    MotorState st;

    // 支路一：word0 置 bit8(超速) + bit9(紧急停机) -> byte0|byte1<<8 = 0x0300；
    // word3 置 bit8(遥控CAN通信中断报警) -> 0x0100
    uint8_t d1[8] = {0x00, 0x03, 0, 0, 0, 0, 0x00, 0x01};
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d1, 8, st));
    CHECK(st.branch1.valid);
    CHECK(st.branch1.word[0] == 0x0300);
    CHECK(st.branch1.word[3] == 0x0100);
    CHECK(st.branch1.faults.size() == 2);
    CHECK_STREQ(st.branch1.faults[0], "超速故障");
    CHECK_STREQ(st.branch1.faults[1], "紧急停机故障");
    CHECK(st.branch1.alarms.size() == 1);
    CHECK_STREQ(st.branch1.alarms[0], "遥控CAN通信中断报警");

    // 支路一 word0 bit7 = 筒内空气过温故障
    uint8_t d2[8] = {0x80, 0x00, 0, 0, 0, 0, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d2, 8, st));
    CHECK(st.branch1.faults.size() == 1);
    CHECK_STREQ(st.branch1.faults[0], "筒内空气过温故障");

    // 支路二：同 bit7 为预留，不应产生故障描述
    uint8_t d3[8] = {0x80, 0x00, 0, 0, 0, 0, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT2, d3, 8, st));
    CHECK(st.branch2.valid);
    CHECK(st.branch2.faults.empty());
    CHECK(st.branch2.alarms.empty());

    // 支路二 word1：bit0(逆变A1相驱动1上管) + word2 bit3(母线电压过压)
    uint8_t d4[8] = {0, 0, 0x01, 0x00, 0x08, 0x00, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT2, d4, 8, st));
    CHECK(st.branch2.word[1] == 0x0001);
    CHECK(st.branch2.word[2] == 0x0008);
    CHECK(st.branch2.faults.size() == 2);
    CHECK_STREQ(st.branch2.faults[0], "逆变A1相驱动1(上管)故障");
    CHECK_STREQ(st.branch2.faults[1], "母线电压过压故障");

    // 冗余 CAN 同样可解码且归入对应支路
    MotorState st2;
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1_RED, d1, 8, st2));
    CHECK(st2.branch1.valid && !st2.branch2.valid);
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT2_RED, d4, 8, st2));
    CHECK(st2.branch2.valid && st2.branch2.faults.size() == 2);
}

//--------------------------------------------------------------------------
// 6. 故障描述查表

static void testFaultTable() {
    std::vector<std::string> out;

    // word1 bit12/13/14/15
    CHECK(motorFaultTexts(1, 1, (1u << 12) | (1u << 13) | (1u << 14) | (1u << 15), out) == 4);
    CHECK_STREQ(out[0], "硬件总故障");
    CHECK_STREQ(out[1], "驱动总故障");
    CHECK_STREQ(out[2], "采样总故障");
    CHECK_STREQ(out[3], "24V欠压故障");

    // word3 全位 16 条
    CHECK(motorFaultTexts(1, 3, 0xFFFF, out) == 16);
    CHECK_STREQ(out[0], "直流母线1电压偏高报警");
    CHECK_STREQ(out[15], "协调光纤报警");

    // 无置位
    CHECK(motorFaultTexts(1, 0, 0, out) == 0);

    // 非法参数
    CHECK(motorFaultTexts(0, 0, 1, out) == 0);
    CHECK(motorFaultTexts(3, 0, 1, out) == 0);
    CHECK(motorFaultTexts(1, 9, 1, out) == 0);
}

//--------------------------------------------------------------------------
// 7. 部分更新合并语义（故障帧重复上报，故障清除后描述随之清除）

static void testPartialUpdate() {
    MotorState st;
    uint8_t d1[8] = {0x01, 0x00, 0, 0, 0, 0, 0, 0};  // 支路一 CPLD自检故障 (bit0)
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d1, 8, st));
    CHECK(st.branch1.faults.size() == 1);
    CHECK_STREQ(st.branch1.faults[0], "CPLD自检故障标志");

    // 复位后故障字清零 -> 描述清空
    uint8_t d2[8] = {0, 0, 0, 0, 0, 0, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_FAULT1, d2, 8, st));
    CHECK(st.branch1.faults.empty());

    // 状态帧不影响支路有效标志
    uint8_t d3[8] = {1, 0, 0, 0, 0, 0, 0, 0};
    CHECK(decodeMotorFrame(MOTOR_CAN_STATE3, d3, 8, st));
    CHECK(st.feedbackSpeed == 1);
    CHECK(st.branch1.valid);   // 仍为 true（部分更新语义）
}

int main() {
    testCmdEncode();
    testCanId();
    testState3Decode();
    testState4Decode();
    testFaultDecode();
    testFaultTable();
    testPartialUpdate();

    std::printf("\n==== pmotorTest: %d passed, %d failed ====\n", g_pass, g_fail);
    return g_fail ? 1 : 0;
}