#include "car.h"
#include <QApplication>
#include <QDebug>
#include <QSerialPort>
#include <QSettings>
#include <QtGlobal>
#include <cstring>

namespace {

// ===================== 线格式 =====================
// 一帧 = 32 位帧头 + 32 位 CAN ID + DLC + 8 字节数据 + 8 位校验和，共 18 字节。
// 多字节字段按主机小端排列，与车载端网关一致；帧头用于在字节流中定位报文起始。
#pragma pack(push, 1)
struct CanFrame {
    quint32 header;      // 固定帧头 0x5AA5FF00（线上字节序 00 FF A5 5A）
    quint32 canId;       // 29 位扩展帧 ID，直接用于分流
    quint8  dlc;         // 数据长度码，固定 8
    quint8  data[8];     // 8 字节 CAN 原生数据段
    quint8  checksum;    // canId 4 字节 + dlc + data[8]，共 13 字节求和取低 8 位
};
#pragma pack(pop)

const quint32 kFrameHeader = 0x5AA5FF00u;
const int     kFrameLen    = int(sizeof(CanFrame));
const int     kDataLen     = 8;

quint8 canChecksum(quint32 canId, quint8 dlc, const quint8* data)
{
    quint8 sum = 0;
    for (int i = 0; i < 4; ++i) sum += quint8((canId >> (8 * i)) & 0xFF);
    sum += dlc;
    for (int i = 0; i < kDataLen; ++i) sum += data[i];
    return sum;
}

// ===================== CAN ID（sheet RC） =====================
// 分类一 遥控器控制指令（上行，7 条）
const quint32 kIdChassisCmd  = 0x18208081;
const quint32 kIdArmCmd1     = 0x18218081;
const quint32 kIdArmCmd2     = 0x18228081;
const quint32 kIdDecontamCmd = 0x18238081;
const quint32 kIdDecontamDbg = 0x18248081;
const quint32 kIdDetectorCmd = 0x18258081;
const quint32 kIdEmergency   = 0x18268081;
// 分类二/三 VCU 状态反馈（下行，12 条）
const quint32 kIdVehicleFb1  = 0x18308180;   // 底盘状态1 BMS
const quint32 kIdVehicleFb2  = 0x18318180;   // 底盘状态2 BMS
const quint32 kIdArmFb1      = 0x18328180;   // 机械臂状态1 末端位置
const quint32 kIdArmFb2      = 0x18338180;   // 机械臂状态2 末端姿态/速度
const quint32 kIdArmFb3      = 0x18348180;   // 机械臂状态3 关节角 J1~J4
const quint32 kIdArmFb4      = 0x18358180;   // 机械臂状态4 关节角 J5~J6
const quint32 kIdDecontamFb1 = 0x18368180;   // 洗消工作状态1
const quint32 kIdDecontamFb2 = 0x18378180;   // 洗消工作状态2
const quint32 kIdDetectorFb1 = 0x18388180;   // 检测仪结果状态1
const quint32 kIdDetectorFb2 = 0x18398180;   // 检测仪结果状态2
const quint32 kIdVehicleFb3  = 0x183A8180;   // 底盘状态3 EPS/制动故障位
const quint32 kIdVehicleFb4  = 0x183B8180;   // 底盘状态4 挡位/驻车/方向盘/制动电机

// ===================== 位运算 =====================
// 分类一/二/三全部是扩展帧、信号一律 Intel（小端）：起始位即最低位序号，
// 位序由低到高、可跨字节（如 bit47 长度 2 就横跨 byte5/byte6）。
quint64 intelBits(const quint8* d, int start, int len)
{
    quint64 v = 0;
    for (int i = 0; i < len; ++i) {
        const int bit = start + i;
        if (d[bit >> 3] & (1u << (bit & 7)))
            v |= quint64(1) << i;
    }
    return v;
}

qint64 intelSigned(const quint8* d, int start, int len)
{
    const quint64 raw = intelBits(d, start, len);
    const quint64 signBit = quint64(1) << (len - 1);
    return qint64((raw ^ signBit) - signBit);   // 符号扩展
}

void setIntelBits(quint8* d, int start, int len, quint64 v)
{
    for (int i = 0; i < len; ++i) {
        const int bit = start + i;
        const quint8 mask = quint8(1u << (bit & 7));
        if (v & (quint64(1) << i)) d[bit >> 3] |= mask;
        else                       d[bit >> 3] &= quint8(~mask);
    }
}

// 物理值 = 总线原始整数值 × 精度 + 偏移量
double phys(quint64 raw, double scale, double offset = 0.0)
{
    return double(raw) * scale + offset;
}

// ===================== 协议配置文件 =====================
// 与 AppContext 用同一份 config.ini（程序目录）。
QString cfgPath()
{
    return QApplication::applicationDirPath() + "/config.ini";
}

// ===================== 故障码 → 中文 =====================
struct CodeText { quint8 code; const char* text; };

const char* lookup(const CodeText* tbl, int n, quint8 code)
{
    for (int i = 0; i < n; ++i)
        if (tbl[i].code == code) return tbl[i].text;
    return nullptr;
}

const CodeText kMcuFaults[] = {
    {6,  "电机缺相预警"},        {17, "控制器过温警告"},      {18, "电机过温警告"},
    {19, "控制器温度传感器警告"}, {20, "电机温度传感器警告"},  {21, "控制器过载警告"},
    {22, "电机超速警告"},        {23, "直流母线电压过压警告"}, {24, "直流母线电压欠压警告"},
    {35, "电流霍尔传感U相器故障"}, {36, "电流霍尔传感W相器故障"}, {37, "VCU报文丢失故障"},
    {38, "VCU循环计数错误"},     {39, "VCU校验和错误"},       {40, "CAN BussOff故障"},
    {49, "母线硬件过压故障"},     {50, "相电流硬件过流故障"},   {51, "IGBT短路故障"},
    {52, "驱动欠压故障"},        {53, "旋变故障"},           {54, "电机超速故障"},
    {55, "低压电源过压故障"},     {56, "低压电源欠压故障"},     {57, "直流母线电压过压故障"},
    {58, "直流母线电压欠压故障"}, {59, "控制器过温故障"},       {60, "电机过温故障"},
    {61, "电机缺相故障"},        {62, "驻坡失效故障"},
};

const CodeText kAirCompFaults[] = {
    {2,  "加速过流"},   {3,  "减速过流"},   {4,  "恒速过流"},   {5,  "加速过压"},
    {6,  "减速过压"},   {7,  "恒速过压"},   {8,  "缓冲过载"},   {9,  "欠压"},
    {10, "控制器过载"}, {11, "电机过载"},   {12, "输入缺相"},   {13, "输出缺相"},
    {14, "模块过热"},   {15, "外部故障"},   {16, "通信异常"},   {17, "接触器异常"},
    {18, "电流检测故障"}, {19, "自学习故障"}, {20, "编码器故障"}, {21, "参数读写异常"},
    {22, "变频器硬件故障"}, {23, "电机对地短路故障"}, {31, "PID反馈丢失"},
    {40, "快速限流超时故障"}, {42, "速度偏差过大"}, {43, "电机超速"}, {45, "电机过温"},
    {90, "编码器线数设定错误"}, {91, "未接编码器"}, {92, "初始位置错误"}, {94, "速度反馈错误"},
};

const CodeText kPlungerFaults[] = {
    {1, "短路故障"}, {2, "欠压"}, {3, "过压"}, {4, "控制器过温"},
    {5, "控制器温度传感器故障"}, {6, "电机过温"}, {7, "电机温度传感器故障"},
};

const CodeText kCommFaults[] = {
    {1, "底盘电机通信异常"}, {2, "底盘BMS通信异常"}, {3, "机械臂通信异常"},
    {4, "空压机通信异常"},   {5, "柱塞泵通信异常"},   {6, "叶片泵通信异常"},
    {7, "气体检测仪通信异常"}, {8, "遥控器通信异常"},
};

const CodeText kDetectorWarns[] = {
    {1, "进气嘴未接"}, {2, "氢气流量不足"}, {4, "仪器故障"}, {8, "浓度过高"},
};

// 底盘状态3（0x183A8180）的 46 个 1 bit 故障标志（bit0~bit45）
const char* const kVsb3Bits[46] = {
    "EPS转子角未对中", "EPS转子角度断开", "EPS转子角度SPI故障", "EPSEEPROM故障",
    "EPS方向盘中点未对中", "EPS方向盘角度超限", "EPS方向盘小齿信号断开", "EPS方向盘中齿信号断开",
    "EPS过电流故障", "EPS电机A相电流异常", "EPS电机C相电流异常", "EPS电机B相电流异常",
    "EPSVCU信号丢失", "EPS电机无助力", "EPS预驱配置失败", "EPS半堵转保护",
    "EPS温度热保护", "EPS电压管理保护", "EPS转角响应不到位", "EPS转子角度PWM故障",
    "EPS电压校验异常", "EPS热敏电阻开路或短路", "EPS热敏电阻温差过大",
    "制动器过载故障", "制动器过温或过压故障", "制动器过流故障", "制动器电源欠压故障",
    "制动器电源过压故障", "制动器压力不足", "制动器缺相故障", "制动器通讯故障",
    "制动器电流采样故障", "制动器驱动故障", "制动器磁编故障", "制动器压力传感器故障",
    "制动器踏板位置传感器故障", "制动器机械故障", "制动器堵转故障", "制动器急停按键闭合警告",
    "制动器驻车时间过长", "制动器CAN1中断警告", "制动器CAN2中断警告", "制动器点火线断开警告",
    "制动器电流异常警告", "制动器温度过高警告", "制动器输入电压过高警告",
};

// 各路 *_FaultLevel 统一为 0 正常 / 1~3 故障等级
const char* const kLevels[4] = {"", "一级故障", "二级故障", "三级故障"};

// 制动电机状态（Brake_*_Mot_fault）：0x00~0x07
const char* const kBrakeMotFaults[8] = {
    "", "电机开路", "电机短路", "制动执行超时", "解除制动执行超时",
    "驱动电路故障", "供电电压过低", "供电电压过高",
};

void addFault(QVector<CanFault>& out, const QString& system, const char* codePrefix,
              quint8 code, const char* text)
{
    if (!text) return;
    CanFault f;
    f.system = system;
    f.code = QString::fromLatin1(codePrefix) + QString::number(code);
    f.desc = QString::fromUtf8(text);
    out.append(f);
}

// 未在码表中登记的码也上报，避免“有故障但界面无提示”
void addFaultByCode(QVector<CanFault>& out, const QString& system, const char* codePrefix,
                    const CodeText* tbl, int n, quint8 code)
{
    if (code == 0) return;
    const char* t = lookup(tbl, n, code);
    if (t) addFault(out, system, codePrefix, code, t);
    else {
        CanFault f;
        f.system = system;
        f.code = QString::fromLatin1(codePrefix) + QString::number(code);
        f.desc = QString::fromLatin1(codePrefix) + QString::number(code);
        out.append(f);
    }
}

// 故障等级：>0 才上报，描述形如“单体电压过低二级故障”
void addLevelFault(QVector<CanFault>& out, const QString& system, const char* codePrefix,
                   const char* name, quint8 level)
{
    if (level == 0 || level > 3) return;
    CanFault f;
    f.system = system;
    f.code = QString::fromLatin1(codePrefix) + QString::number(level);
    f.desc = QString::fromUtf8(name) + QString::fromUtf8(kLevels[level]);
    out.append(f);
}

const char* const kSysChassis  = "机器人底盘";
const char* const kSysWash     = "洗消系统";
const char* const kSysDetector = "气体检测仪";

} // namespace

// ===================== 构造与生命周期 =====================
CarCommunication::CarCommunication(Telemetry* tel, QObject* parent)
    : QObject(parent), m_tel(tel)
{
    m_serial = new QSerialPort(this);
    connect(m_serial, &QSerialPort::readyRead, this, &CarCommunication::onReadyRead);

    // 串口配置与 AppContext 共用 config.ini（默认 /dev/ttyS7，500000 8N1）
    QSettings s(cfgPath(), QSettings::IniFormat);
    m_device = s.value("car/device", "/dev/ttyS7").toString();
    m_baud   = s.value("car/baud", 500000).toInt();
    m_selfCheckFault = s.value("net/selfcheck_fault", true).toBool();

    // 洗消四参数按存盘值初始化，否则开机帧发的是硬编码初值而非用户设定值。
    // INI 读回的是文本（"6.5"），须用 toDouble 解析；协议字段都是整数，四舍五入取整。
    m_plungerFlow = qRound(qBound(0.0, s.value("param/pump_flow",    20).toDouble(),  24.0));
    m_foamRatio   = qRound(qBound(6.0, s.value("param/foam_ratio",    6).toDouble(),  12.0));
    m_foamPercent = qRound(qBound(0.0, s.value("param/foam_percent",  3).toDouble(),  10.0));
    m_airFlow     = qRound(qBound(0.0, s.value("param/air_flow",    100).toDouble(), 340.0));

    // 通信超时判定：最慢的下行报文周期 1000ms，连续 2s 无任何合法帧 → 通信异常
    m_hbTimer = new QTimer(this);
    m_hbTimer->setInterval(2000);
    connect(m_hbTimer, &QTimer::timeout, this, &CarCommunication::onHeartbeatTimeout);

    // 上行周期定时器，周期取自协议「报文周期时间」列
    m_fastTimer = new QTimer(this);   // 50ms：底盘 / 机械臂 / 急停
    m_fastTimer->setInterval(50);
    connect(m_fastTimer, &QTimer::timeout, this, &CarCommunication::onSendFast);

    m_midTimer = new QTimer(this);    // 100ms：洗消指令 / 洗消调试
    m_midTimer->setInterval(100);
    connect(m_midTimer, &QTimer::timeout, this, &CarCommunication::onSendMid);

    m_slowTimer = new QTimer(this);   // 1000ms：气体检测仪
    m_slowTimer->setInterval(1000);
    connect(m_slowTimer, &QTimer::timeout, this, &CarCommunication::onSendSlow);

    // 本端自检序列（见 onSelfCheckTick 的 TODO）
    m_selfCheckTimer = new QTimer(this);
    m_selfCheckTimer->setInterval(450);
    connect(m_selfCheckTimer, &QTimer::timeout, this, &CarCommunication::onSelfCheckTick);

    if (m_tel) m_tel->js.resize(6);
}

CarCommunication::~CarCommunication()
{
    stop();
}

bool CarCommunication::start()
{
    if (m_serial->isOpen())
        return true;

    m_serial->setPortName(m_device);
    m_serial->setBaudRate(m_baud);
    m_serial->setDataBits(QSerialPort::Data8);
    m_serial->setStopBits(QSerialPort::OneStop);
    m_serial->setParity(QSerialPort::NoParity);
    m_serial->setFlowControl(QSerialPort::NoFlowControl);

    if (!m_serial->open(QIODevice::ReadWrite)) {
        // 开发机（Windows）没有 /dev/ttyS7 时走这里：不启动定时器，界面显示通信异常
        qWarning() << "车端串口打开失败:" << m_device << m_serial->errorString();
        setCommOk(false);
        return false;
    }
    m_buf.clear();
    m_hbTimer->start();
    m_fastTimer->start();
    m_midTimer->start();
    m_slowTimer->start();
    return true;
}

void CarCommunication::stop()
{
    m_hbTimer->stop();
    m_fastTimer->stop();
    m_midTimer->stop();
    m_slowTimer->stop();
    m_selfCheckTimer->stop();
    if (m_serial->isOpen()) m_serial->close();
    m_buf.clear();
}

void CarCommunication::setCommOk(bool ok)
{
    if (ok == m_lastCommOk) return;
    m_lastCommOk = ok;
    if (m_tel) m_tel->commOk = ok;
    emit commOkChanged(ok);
}

// ===================== 收包与分流 =====================
void CarCommunication::onReadyRead()
{
    m_buf.append(m_serial->readAll());

    // 串口是字节流：逐字节找帧头，校验不过则右移一字节继续找（容忍粘包、拆包与杂包）。
    // 尾部不足一帧的残段留在 m_buf 里等下次数据。
    int off = 0;
    while (off + kFrameLen <= m_buf.size()) {
        CanFrame f;
        std::memcpy(&f, m_buf.constData() + off, kFrameLen);
        if (f.header == kFrameHeader && f.dlc == kDataLen
            && f.checksum == canChecksum(f.canId, f.dlc, f.data)) {
            handleFrame(f.canId, f.data);
            off += kFrameLen;
        } else {
            ++off;
        }
    }
    if (off > 0) m_buf.remove(0, off);

    // 防御：长期收不到帧头时缓冲会一直涨，超过上限只保留尾部一帧长度
    if (m_buf.size() > 4096) m_buf.remove(0, m_buf.size() - kFrameLen);
}

void CarCommunication::handleFrame(quint32 canId, const quint8* d)
{
    if (!m_tel) return;

    // 收到任意合法帧即刷新链路（CAN 无独立心跳报文）
    m_hbTimer->start();
    setCommOk(true);

    switch (canId) {
    // ---------- 0x18308180 底盘状态1（BMS）：电池总况与故障等级 ----------
    case kIdVehicleFb1: {
        m_tel->volt = phys(intelBits(d, 0, 16), 0.1);
        m_tel->soc  = int(phys(intelBits(d, 16, 16), 0.1));

        m_vsf1Faults.clear();
        addFaultByCode(m_vsf1Faults, QString::fromUtf8(kSysChassis), "MCU",
                       kMcuFaults, int(sizeof(kMcuFaults) / sizeof(CodeText)),
                       quint8(intelBits(d, 32, 8)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "MCUL",
                      "底盘电机", quint8(intelBits(d, 40, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "CELLL",
                      "单体电压过低", quint8(intelBits(d, 45, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "CELLHL",
                      "单体电压过高", quint8(intelBits(d, 47, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "VOLTL",
                      "总电压过低", quint8(intelBits(d, 49, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "VOLTHL",
                      "总电压过高", quint8(intelBits(d, 51, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "DLTPL",
                      "放电低温", quint8(intelBits(d, 53, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "DHTPL",
                      "放电高温", quint8(intelBits(d, 55, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "CLTPL",
                      "充电低温", quint8(intelBits(d, 57, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "CHTPL",
                      "充电高温", quint8(intelBits(d, 59, 2)));
        addLevelFault(m_vsf1Faults, QString::fromUtf8(kSysChassis), "TIL",
                      "温度不均衡", quint8(intelBits(d, 61, 2)));
        break;
    }

    // ---------- 0x18318180 底盘状态2（BMS）：手刹 / 车速 / 继电器 / 通信 ----------
    case kIdVehicleFb2: {
        m_vsf2Faults.clear();
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "VIL",
                      "压差不均衡", quint8(intelBits(d, 0, 2)));
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "DOCL",
                      "放电过流", quint8(intelBits(d, 2, 2)));
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "COCL",
                      "充电过流", quint8(intelBits(d, 4, 2)));
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "INCL",
                      "内部通讯", quint8(intelBits(d, 6, 2)));
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "EXCL",
                      "外部通讯", quint8(intelBits(d, 8, 2)));
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "SOCL",
                      "SOC过低", quint8(intelBits(d, 10, 2)));
        addLevelFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), "INSL",
                      "绝缘", quint8(intelBits(d, 12, 2)));

        // 继电器粘连：0=正常 非 0=故障
        struct { int bit; const char* name; const char* code; } relays[] = {
            {14, "加热继电器粘连", "HEATW"},
            {16, "充电继电器粘连", "CHGW"},
            {18, "总负继电器粘连", "NEGW"},
            {20, "总正继电器粘连", "POSW"},
        };
        for (const auto& r : relays) {
            if (intelBits(d, r.bit, 2) != 0)
                addFault(m_vsf2Faults, QString::fromUtf8(kSysChassis), r.code, 1, r.name);
        }

        m_tel->handbrake = intelBits(d, 22, 2) != 0;
        // Vehi_spd：协议表为 8 位、精度 0.01（0~2.55 km/h）。其后 Vehi_RdyStatus(32)、
        // Reserve(34)、All_Comm_FaultCode(40) 的起始位与 8 位布局自洽，故按协议取值。
        m_tel->speed = phys(intelBits(d, 24, 8), 0.01);
        m_tel->rdyStatus = int(intelBits(d, 32, 2));

        const quint8 comm = quint8(intelBits(d, 40, 8));   // All_Comm_FaultCode
        addFaultByCode(m_vsf2Faults, QString::fromUtf8(kSysChassis), "COMM",
                       kCommFaults, int(sizeof(kCommFaults) / sizeof(CodeText)), comm);
        break;
    }

    // ---------- 0x183A8180 底盘状态3：EPS / 制动故障位 ----------
    case kIdVehicleFb3: {
        m_vsf3Faults.clear();
        for (int i = 0; i < 46; ++i) {
            if (intelBits(d, i, 1) != 0)
                addFault(m_vsf3Faults, QString::fromUtf8(kSysChassis), "EPS", quint8(i),
                         kVsb3Bits[i]);
        }
        const quint8 eps = quint8(intelBits(d, 48, 8));
        if (eps != 0) {
            CanFault f;
            f.system = QString::fromUtf8(kSysChassis);
            f.code = QString("EPSC") + QString::number(eps);
            f.desc = QString("EPS故障码") + QString::number(eps);
            m_vsf3Faults.append(f);
        }
        break;
    }

    // ---------- 0x183B8180 底盘状态4：挡位 / 驻车 / 方向盘 / 制动电机 ----------
    case kIdVehicleFb4: {
        m_vsf4Faults.clear();
        m_tel->gearPos    = int(intelBits(d, 0, 2));   // 0=N 1=D 2=R
        m_tel->parking    = int(intelBits(d, 2, 2));   // 0=未驻车 1=左电机 2=右电机 3=完全驻车
        m_tel->awdMode    = int(intelBits(d, 4, 2));   // 0=二驱 1=四驱 2=四驱锁定
        m_tel->steerAngle = double(intelBits(d, 8, 16));

        const quint8 align = quint8(intelBits(d, 24, 8));
        if (align == 0x11) addFault(m_vsf4Faults, QString::fromUtf8(kSysChassis), "ALIGN", 1, "方向盘对中失败");
        else if (align != 0xEE && align != 0x55)
            addFault(m_vsf4Faults, QString::fromUtf8(kSysChassis), "ALIGN", 0, "方向盘未对中");

        // 注：协议表把两个信号的“信号值描述”写反了（Brake_Right 描述成左电机、
        // Brake_Left 描述成右电机），此处以信号名为准。
        const quint8 br = quint8(intelBits(d, 32, 8));
        if (br != 0) addFault(m_vsf4Faults, QString::fromUtf8(kSysChassis), "BRKR", br,
                              br < 8 ? kBrakeMotFaults[br] : "制动电机故障");
        const quint8 bl = quint8(intelBits(d, 40, 8));
        if (bl != 0) addFault(m_vsf4Faults, QString::fromUtf8(kSysChassis), "BRKL", bl,
                              bl < 8 ? kBrakeMotFaults[bl] : "制动电机故障");
        break;
    }

    // ---------- 0x18328180 机械臂状态1：末端位置 ----------
    case kIdArmFb1: {
        m_tel->pos = QString("X:%1 Y:%2 Z:%3")
                         .arg(intelSigned(d, 0, 16))
                         .arg(intelSigned(d, 16, 16))
                         .arg(intelSigned(d, 32, 16));
        break;
    }
    // ---------- 0x18338180 机械臂状态2：末端姿态与末端速度 ----------
    case kIdArmFb2: {
        m_tel->pose = QString("RX:%1 RY:%2 RZ:%3")
                          .arg(intelSigned(d, 0, 16))
                          .arg(intelSigned(d, 16, 16))
                          .arg(intelSigned(d, 32, 16));
        m_tel->vel = double(intelSigned(d, 48, 8));
        break;
    }
    // ---------- 0x18348180 机械臂状态3：关节角 J1~J4 ----------
    case kIdArmFb3: {
        if (m_tel->js.size() < 6) m_tel->js.resize(6);
        for (int i = 0; i < 4; ++i)
            m_tel->js[i] = double(intelSigned(d, i * 16, 16));
        break;
    }
    // ---------- 0x18358180 机械臂状态4：关节角 J5~J6 ----------
    case kIdArmFb4: {
        if (m_tel->js.size() < 6) m_tel->js.resize(6);
        m_tel->js[4] = double(intelSigned(d, 0, 16));
        m_tel->js[5] = double(intelSigned(d, 16, 16));
        break;
    }

    // ---------- 0x18368180 洗消工作状态1：模式 / 泵 / 球阀 / 液位 / 流量 ----------
    case kIdDecontamFb1: {
        const quint8 mode = quint8(intelBits(d, 0, 2));
        m_tel->washMode = (mode == 3) ? QString::fromUtf8("消毒作业")
                        : (mode == 2) ? QString::fromUtf8("消除作业")
                                      : QString("--");
        m_tel->washStart = intelBits(d, 2, 1) != 0;
        m_tel->plunger = intelBits(d, 3, 1) != 0;
        m_tel->air     = intelBits(d, 4, 1) != 0;
        m_tel->leaf    = intelBits(d, 5, 1) != 0;

        m_tel->d1open = intelBits(d, 8, 1) != 0;
        m_tel->d1close = !m_tel->d1open;
        m_tel->d2open = intelBits(d, 9, 1) != 0;
        m_tel->d2close = !m_tel->d2open;

        m_washFaults.clear();
        if (intelBits(d, 10, 1) != 0)   // Sys_fault：无分路码，只报总故障
            addFault(m_washFaults, QString::fromUtf8(kSysWash), "SYS", 1, "洗消系统故障");
        if (intelBits(d, 6, 1) != 0)
            addFault(m_washFaults, QString::fromUtf8(kSysWash), "LVL", 1, "A组分/清水液位低");
        if (intelBits(d, 7, 1) != 0)
            addFault(m_washFaults, QString::fromUtf8(kSysWash), "LVL", 2, "B组分液位低");

        m_tel->levelA    = double(intelBits(d, 16, 8));
        m_tel->levelFoam = double(intelBits(d, 24, 8));
        m_tel->flowA     = double(intelBits(d, 32, 8));
        m_tel->flowB     = double(intelBits(d, 40, 8));
        m_tel->gasFlow   = double(intelBits(d, 48, 8));
        m_tel->foam      = double(intelBits(d, 56, 8));
        break;
    }
    // ---------- 0x18378180 洗消工作状态2：压力 / 流量反馈 / 泵故障码 ----------
    case kIdDecontamFb2: {
        m_tel->press    = phys(intelBits(d, 0, 8), 0.1);
        m_tel->airPress = phys(intelBits(d, 8, 8), 0.1);
        m_tel->flowPlungerFb = double(intelBits(d, 16, 8));
        m_tel->flowAirFb     = double(intelBits(d, 32, 8));
        m_tel->flowVaneFb    = double(intelBits(d, 40, 8));

        m_pumpFaults.clear();
        addFaultByCode(m_pumpFaults, QString::fromUtf8(kSysWash), "AIR",
                       kAirCompFaults, int(sizeof(kAirCompFaults) / sizeof(CodeText)),
                       quint8(intelBits(d, 48, 8)));
        addFaultByCode(m_pumpFaults, QString::fromUtf8(kSysWash), "PLG",
                       kPlungerFaults, int(sizeof(kPlungerFaults) / sizeof(CodeText)),
                       quint8(intelBits(d, 56, 8)));
        break;
    }

    // ---------- 0x18388180 检测仪结果状态1：状态 / 毒物种类与等级 / 告警 ----------
    case kIdDetectorFb1: {
        const quint8 sta = quint8(intelBits(d, 0, 8));
        const quint8 warn = quint8(intelBits(d, 24, 8));
        // 2=计算目标基线 3=等待稳定 4=检测 5=等待点火 6=确定初始基线
        m_tel->gasHost = (warn & 0x04) ? QString("fault")
                       : (sta == 4)    ? QString("detecting")
                                       : QString("standby");

        // Toxic_types 是位组合：1=P类 2=S类 4=HNO类 8=CH类 16=AS类
        const quint8 types = quint8(intelBits(d, 8, 8));
        static const struct { quint8 bit; const char* name; } kTypes[] = {
            {1, "P类"}, {2, "S类"}, {4, "HNO类"}, {8, "CH类"}, {16, "AS类"},
        };
        QStringList names;
        for (const auto& t : kTypes)
            if (types & t.bit) names << QString::fromUtf8(t.name);
        m_tel->gasType = names.isEmpty() ? QString("--") : names.join("/");

        const quint8 lvl = quint8(intelBits(d, 16, 8));
        m_tel->gasLevel = (lvl >= 1 && lvl <= 5) ? QString("%1级").arg(lvl) : QString("--");

        m_detectorFaults.clear();
        for (const CodeText& w : kDetectorWarns)
            if (warn & w.code)
                m_detectorFaults.append(CanFault{QString::fromUtf8(kSysDetector),
                                                 QString::fromLatin1("W") + QString::number(w.code),
                                                 QString::fromUtf8(w.text)});
        break;
    }
    // ---------- 0x18398180 检测仪结果状态2：40 位浓度（ppb） ----------
    case kIdDetectorFb2: {
        m_tel->gasConc = double(intelBits(d, 0, 40));
        break;
    }

    default:
        return;   // 未知 canId 不触发刷新（分类四本次未实现）
    }

    cacheFaults();
    emit telemetryUpdated();
}

// 把各帧的故障合并进 Telemetry，并把“新出现”的故障写入故障历史
void CarCommunication::cacheFaults()
{
    if (!m_tel) return;

    const QVector<QVector<CanFault>*> groups = {
        &m_vsf1Faults, &m_vsf2Faults, &m_vsf3Faults, &m_vsf4Faults,
    };
    QStringList chassis;
    for (const QVector<CanFault>* g : groups)
        for (const CanFault& f : *g) chassis << f.desc;
    m_tel->chassisFaults = chassis;

    QStringList wash;
    for (const CanFault& f : m_washFaults) wash << f.desc;
    for (const CanFault& f : m_detectorFaults) wash << f.desc;
    m_tel->washFaults = wash;

    QStringList pump;
    for (const CanFault& f : m_pumpFaults) pump << f.desc;
    m_tel->pumpFaults = pump;

    // 周期报文会反复报到同一故障，只在首次出现时写故障历史
    const QVector<QVector<CanFault>*> all = {
        &m_vsf1Faults, &m_vsf2Faults, &m_vsf3Faults, &m_vsf4Faults,
        &m_washFaults, &m_pumpFaults, &m_detectorFaults,
    };
    for (const QVector<CanFault>* g : all) {
        for (const CanFault& f : *g) {
            const QString key = f.system + "|" + f.code;
            if (m_reportedFaults.contains(key)) continue;
            m_reportedFaults.insert(key);
            emit faultAppended(f.system, f.code, f.desc);
        }
    }
}

// ===================== 发包 =====================
void CarCommunication::sendFrame(quint32 canId, const quint8* data)
{
    if (!m_serial->isOpen()) return;

    CanFrame f;
    f.header = kFrameHeader;
    f.canId = canId;
    f.dlc = kDataLen;
    std::memcpy(f.data, data, kDataLen);
    f.checksum = canChecksum(canId, f.dlc, f.data);
    m_serial->write(reinterpret_cast<const char*>(&f), kFrameLen);
}

// 50ms 组：Chassis_ControlCmd / Arm_ControlCmd1 / Arm_ControlCmd2 / Emergency_Cmd
void CarCommunication::onSendFast()
{
    {
        quint8 d[8] = {0};
        setIntelBits(d, 0, 1, 1);                // Chassis_modeCmd 有效
        setIntelBits(d, 1, 2, 0);                // MCU_modeCmd 底盘预留
        setIntelBits(d, 3, 1, 1);                // MCU_enable 有效
        setIntelBits(d, 4, 4, 0);                // Reserve 置 0
        setIntelBits(d, 8, 16, m_gear);
        setIntelBits(d, 24, 16, m_targetSpd);
        setIntelBits(d, 40, 16, m_turning);
        sendFrame(kIdChassisCmd, d);
    }
    {
        quint8 d[8] = {0};
        setIntelBits(d, 0, 1, m_armMode ? 1 : 0);   // Arm_modeCmd
        setIntelBits(d, 1, 1, m_armMode ? 1 : 0);   // Arm_enable
        setIntelBits(d, 2, 6, 0);                   // Reserve
        setIntelBits(d, 8, 16, m_updown);
        setIntelBits(d, 24, 16, m_leftright);
        setIntelBits(d, 40, 16, m_frontrear);
        sendFrame(kIdArmCmd1, d);
    }
    {
        quint8 d[8] = {0};
        setIntelBits(d, 0, 16, m_roll);
        setIntelBits(d, 16, 16, m_pitch);
        setIntelBits(d, 32, 16, m_yaw);
        setIntelBits(d, 48, 8, m_homingPause);
        sendFrame(kIdArmCmd2, d);
    }
    {
        quint8 d[8] = {0};
        setIntelBits(d, 0, 8, m_emergStop);
        sendFrame(kIdEmergency, d);
    }
}

// 100ms 组：Decontam_ControlCmd / Decontam_DebugCmd
void CarCommunication::onSendMid()
{
    {
        quint8 d[8] = {0};
        setIntelBits(d, 0, 2, m_decontamMode);
        setIntelBits(d, 2, 1, m_decontamStart ? 1 : 0);
        setIntelBits(d, 3, 5, 0);                // Reserve 置 0

        // 发泡倍数 / 泡沫比例 / 空压机流量 / 柱塞泵流量：均由页面保存时经 sendSetParam
        // 写入缓存（构造时先按 config.ini 初始化），这里直接读缓存组帧，不再每帧读文件。
        // 取值范围按协议「信号值描述」列收口：发泡倍数 6-12 倍、泡沫比例 0-10%、
        // 空压机流量 0-340 L/min；界面 ParamControl 的可调范围与之一致。
        setIntelBits(d, 8, 8, quint64(qBound(6, m_foamRatio, 12)));
        setIntelBits(d, 16, 8, quint64(qBound(0, m_foamPercent, 10)));
        setIntelBits(d, 24, 8, quint64(qBound(0, m_airFlow, 340)));
        setIntelBits(d, 32, 8, quint64(qBound(0, m_plungerFlow, 24)));
        sendFrame(kIdDecontamCmd, d);
    }
    {
        quint8 d[8] = {0};
        setIntelBits(d, 0, 1, m_debugMode ? 1 : 0);
        setIntelBits(d, 1, 1, m_plungerOn ? 1 : 0);
        setIntelBits(d, 2, 1, m_airOn ? 1 : 0);
        setIntelBits(d, 3, 1, m_vaneOn ? 1 : 0);
        setIntelBits(d, 4, 1, m_d1Open ? 1 : 0);
        setIntelBits(d, 5, 1, m_d2Open ? 1 : 0);
        setIntelBits(d, 6, 2, 0);                // Debug_Reserve1
        setIntelBits(d, 8, 16, quint64(qBound(0, int(m_airRpm), 3000)));
        setIntelBits(d, 24, 16, quint64(qBound(0, int(m_plungerRpm), 1450)));
        setIntelBits(d, 40, 16, quint64(qBound(0, int(m_vaneRpm), 2000)));
        // 界面每个泵只有一组“启动/停止”，上电与启动同源
        setIntelBits(d, 56, 1, m_airOn ? 1 : 0);
        setIntelBits(d, 57, 1, m_plungerOn ? 1 : 0);
        setIntelBits(d, 58, 1, m_vaneOn ? 1 : 0);
        sendFrame(kIdDecontamDbg, d);
    }
}

// 1000ms 组：Detector_ControlCmd
void CarCommunication::onSendSlow()
{
    quint8 d[8] = {0};
    setIntelBits(d, 0, 8, m_detectorCmd);
    sendFrame(kIdDetectorCmd, d);
}

// ===================== 上行命令 → 控制状态 =====================
void CarCommunication::sendChassisMove(const QString& dir, double speed, const QString& turn)
{
    // 档位推杆量：后 0 / 中 2048 / 前 4096
    if (dir == "fwd")       m_gear = 4096;
    else if (dir == "back") m_gear = 0;
    else                    m_gear = kRcAxisMid;

    // 转向推杆量：左 0 / 中 2048 / 右 4096
    if (turn == "left")       m_turning = 0;
    else if (turn == "right") m_turning = 4096;
    else                      m_turning = kRcAxisMid;

    // Vehi_TargetSpd 精度 0.01 km/h，量程 0~500（即 0~5.00 km/h）
    m_targetSpd = quint16(qBound(0, int(qRound(speed * 100.0)), 500));
}

void CarCommunication::sendArmMove(const QString& axis, int dir)
{
    // 轴位推杆量：dir +1 / -1 / 0 → 4096 / 0 / 中位
    const quint16 v = (dir > 0) ? 4096 : (dir < 0) ? 0 : kRcAxisMid;

    if (axis == "z")          m_updown = v;
    else if (axis == "x")     m_leftright = v;
    else if (axis == "y")     m_frontrear = v;
    else if (axis == "roll")  m_roll = v;
    else if (axis == "pitch") m_pitch = v;
    else if (axis == "yaw")   m_yaw = v;
}

void CarCommunication::sendArmReturn()
{
    m_homingPause = 0xAA;   // 系统回位
}

void CarCommunication::sendArmPause()
{
    m_homingPause = 0x55;   // 暂停回位
}

void CarCommunication::sendGasStart()
{
    m_detectorCmd = 0xAA;   // 启动检测
}

void CarCommunication::sendGasStop()
{
    m_detectorCmd = 0x55;   // 停止检测
}

void CarCommunication::sendWashStart(const QString& mode)
{
    m_decontamMode = (mode == "disinfect") ? 3 : 2;   // 3=手动消毒 2=手动消除
    m_decontamStart = true;
}

void CarCommunication::sendWashStop()
{
    m_decontamStart = false;
}

void CarCommunication::sendBallValve(int valve, bool open)
{
    if (valve == 1) m_d1Open = open;
    else            m_d2Open = open;
    m_debugMode = true;   // 球阀属于调试指令
}

void CarCommunication::sendPumpPower(const QString& pump, bool on)
{
    if (pump == "plunger")   m_plungerOn = on;
    else if (pump == "air")  m_airOn = on;
    else if (pump == "leaf") m_vaneOn = on;   // 叶片泵 → VanePump
    m_debugMode = true;
}

void CarCommunication::sendPumpRpm(const QString& pump, int rpm)
{
    if (pump == "plunger")   m_plungerRpm = quint16(qBound(0, rpm, 1450));
    else if (pump == "air")  m_airRpm = quint16(qBound(0, rpm, 3000));
    else if (pump == "leaf") m_vaneRpm = quint16(qBound(0, rpm, 2000));
}

void CarCommunication::sendSetParam(const QString& key, double value)
{
    // 参数缓存与 config.ini 同源：页面保存时先落盘再调这里，周期帧只读缓存。
    if (key == "param/speed")
        m_targetSpd = quint16(qBound(0, int(qRound(value * 100.0)), 500));
    else if (key == "param/pump_flow")
        // 字段是 0~24 的整数，界面刻度是 0.1：必须四舍五入而不是截断，
        // 否则 0.1 累加出的尾差（面板 5.0 实际存 4.9999999999999982）会下发成 4
        m_plungerFlow = qRound(qBound(0.0, value, 24.0));
    else if (key == "param/foam_ratio")
        m_foamRatio = qRound(qBound(6.0, value, 12.0));
    else if (key == "param/foam_percent")
        m_foamPercent = qRound(qBound(0.0, value, 10.0));
    else if (key == "param/air_flow")
        m_airFlow = qRound(qBound(0.0, value, 340.0));
}

void CarCommunication::sendSelfCheckStart()
{
    m_selfCheckIndex = 0;
    m_selfCheckTimer->start();
}

void CarCommunication::sendEstop()
{
    m_emergStop = 0x55;
}

void CarCommunication::sendEstopReset()
{
    m_emergStop = 0;
}

// TODO：CAN 矩阵（sheet RC）没有自检报文，故暂由本端跑完 10 项序列，仅为维持
//       ④自检 → ⑤作业选择 的流程不中断（onSelfCheckDone 是唯一点亮“作业选择”的地方）。
//       拿到真实自检报文后，改为解析该报文并删除此本地驱动。
void CarCommunication::onSelfCheckTick()
{
    bool ok = true;
    if (m_selfCheckFault && m_selfCheckIndex == 6) ok = false;   // 演示：球阀D1关闭故障
    emit selfCheckItem(m_selfCheckIndex, ok);

    ++m_selfCheckIndex;
    if (m_selfCheckIndex >= 10) {
        m_selfCheckTimer->stop();
        emit selfCheckDone(!m_selfCheckFault);
    }
}

void CarCommunication::onHeartbeatTimeout()
{
    setCommOk(false);
}