/**
 * @file ethercat_can_bridge.cpp
 * @brief EtherCAT PDO ↔ CAN 转发桥实现（1.6.21：段 2 电机整轮密排）。
 */
#include "comm/ethercat_can_bridge.h"
#include "device/canfd.h"
#include "device/motor_state.h"

#include "RTE_Components.h"
#include CMSIS_device_header   /* __DMB()：seqlock 的编译器/总线屏障 */

#include <cstring>

volatile uint8_t  EthercatCanBridge::s_lastSeq      = 0U;
volatile bool     EthercatCanBridge::s_seqPrimed    = false;
volatile bool     EthercatCanBridge::s_coldStartPending = false;
volatile uint8_t  EthercatCanBridge::s_lastTxResult = 0U;
volatile uint16_t EthercatCanBridge::s_txCount      = 0U;
volatile uint16_t EthercatCanBridge::s_dropCount    = 0U;

volatile bool     EthercatCanBridge::s_rxValid      = false;
volatile bool     EthercatCanBridge::s_rxPublished  = false;
volatile uint32_t EthercatCanBridge::s_xrxSeq       = 0U;
volatile uint8_t  EthercatCanBridge::s_rxCh         = 0U;
volatile uint8_t  EthercatCanBridge::s_rxDlc        = 0U;
volatile bool     EthercatCanBridge::s_rxTruncated  = false;
volatile uint32_t EthercatCanBridge::s_rxId         = 0U;
volatile uint32_t EthercatCanBridge::s_rxCount      = 0U;
uint8_t           EthercatCanBridge::s_rxData[kEcatCanXRxDataMax] = {0};

uint8_t           EthercatCanBridge::s_motShadow[kEcatMotRegionBytes] = {0};
uint32_t          EthercatCanBridge::s_motPubSeq[kMotorMaxNodes + 1U] = {0};
uint8_t           EthercatCanBridge::s_motPubOnline[kMotorMaxNodes + 1U] = {0};
uint16_t          EthercatCanBridge::s_motPubMask   = 0U;
uint8_t           EthercatCanBridge::s_motPubCount  = 0U;
bool              EthercatCanBridge::s_motShadowValid = false;

namespace {
/** 小端读写：PDO 字节序按 EtherCAT 惯例固定小端。 */
inline uint32_t rd32le(const uint8_t *p)
{
    return (uint32_t)p[0] | ((uint32_t)p[1] << 8) |
           ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}
inline void wr32le(uint8_t *p, uint32_t v)
{
    p[0] = (uint8_t)(v);         p[1] = (uint8_t)(v >> 8);
    p[2] = (uint8_t)(v >> 16);   p[3] = (uint8_t)(v >> 24);
}
inline void wr16le(uint8_t *p, uint16_t v)
{
    p[0] = (uint8_t)(v);  p[1] = (uint8_t)(v >> 8);
}

/** 段 2 头 + count 槽在影子区中的总长度。 */
inline uint16_t motRegionLen(uint8_t count)
{
    return (uint16_t)(kEcatMotHdrSize + (uint16_t)count * kEcatMotSlotSize);
}
}  // namespace

uint8_t EthercatCanBridge::sendBatch(const uint8_t *out)
{
    uint8_t want = out[ECAT_TXB_OFF_COUNT];
    if (want == 0U) {
        return 0U;
    }
    if (want > kEcatTxBatchMax) {
        want = kEcatTxBatchMax;
    }
    const bool stopOnError = (out[ECAT_TXB_OFF_FLAGS] & 0x01U) != 0U;

    uint8_t  sent = 0U;
    uint16_t off  = ECAT_TXB_OFF_ITEMS;

    for (uint8_t i = 0U; i < want; ++i) {
        if ((uint16_t)(off + ECAT_FRM_HDR_SIZE) > kEcatPdoOutBytes) {
            break;
        }
        const uint8_t *f = &out[off];
        uint8_t dlc = f[ECAT_FRM_OFF_DLC];
        if (dlc > kEcatCanFdDataMax) {
            dlc = kEcatCanFdDataMax;
        }
        if ((uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc) > kEcatPdoOutBytes) {
            break;
        }

        const uint8_t ch = f[ECAT_FRM_OFF_CH];
        CanFd::CanTxResult r;
        if (!CanFd::isChannelEnabled(ch)) {
            r = CanFd::CanTxResult::kErrInvalid;
        } else {
            r = CanFd::enqueueTx(ch, rd32le(&f[ECAT_FRM_OFF_ID]),
                                 &f[ECAT_FRM_HDR_SIZE], dlc);
        }

        if (r == CanFd::CanTxResult::kOk) {
            ++s_txCount;
            ++sent;
        } else {
            s_lastTxResult = (uint8_t)r;
            if (stopOnError) {
                break;
            }
        }
        off = (uint16_t)(off + ECAT_FRM_HDR_SIZE + dlc);
    }
    return sent;
}

/**
 * 段 2：电机整轮密排。
 *
 * 1. 扫 MotorState，每个节点号 1..9 在 3 个通道里取 seq 最新的一条；
 * 2. 若节点集合与各节点 seq 均与上一拍发布相同 → 整段复用影子区（跳过重读）；
 * 3. 否则按 node_id 升序密排写入，空节点不占位；seqlock 奇数（写入中）的槽
 *    本拍省略，下拍自愈 —— 不发半新半旧的 48B。
 */
uint8_t EthercatCanBridge::fillMotorRound(uint8_t *in)
{
    const MotorStateTable &mt = MotorState::table();

    /* ── 1. 每节点选一条最新槽 ── */
    const MotorSlot *picks[kMotorMaxNodes];
    uint8_t nodes[kMotorMaxNodes];
    uint8_t n = 0U;

    for (uint8_t node = 1U; node <= kMotorMaxNodes; ++node) {
        const MotorSlot *best = nullptr;
        for (uint8_t ch = 0U; ch < kMotorChannels; ++ch) {
            const MotorSlot &s =
                mt.slots[(uint8_t)((node - 1U) * kMotorChannels + ch)];
            if (s.node_id == 0U) {
                continue;
            }
            if (best == nullptr || s.seq >= best->seq) {
                best = &s;
            }
        }
        if (best != nullptr) {
            picks[n] = best;
            nodes[n] = node;
            ++n;
        }
    }

    /* ── 2. 判「上一轮已填充且未变」──
     * 节点集合一致 + 每节点 seq/online 一致 + 头字段（active/boot）一致 + 写侧
     * seqlock 偶数 → 复用影子。online 必须比：checkOffline 超时置 0 时不 bump
     * seq，只比 seq 会把「已离线」永远报成在线。 */
    bool unchanged = s_motShadowValid && (n == s_motPubCount)
                     && mt.active_count == s_motShadow[ECAT_MOTW_OFF_ACTIVE - ECAT_MOTW_OFF_COUNT]
                     && mt.boot_active   == s_motShadow[ECAT_MOTW_OFF_BOOT   - ECAT_MOTW_OFF_COUNT]
                     && s_motShadow[ECAT_MOTW_OFF_MAGIC - ECAT_MOTW_OFF_COUNT]
                            == kEcatMotLayoutMagic;
    if (unchanged) {
        for (uint8_t i = 0U; i < n; ++i) {
            const uint8_t node = nodes[i];
            const MotorSlot &s = *picks[i];
            if ((s.wr_seq & 1U) != 0U
                || s.seq != s_motPubSeq[node]
                || s.online != s_motPubOnline[node]
                || (s_motPubMask & (uint16_t)(1U << node)) == 0U) {
                unchanged = false;
                break;
            }
        }
    }

    const uint16_t region = motRegionLen(unchanged ? s_motPubCount : n);

    if (unchanged) {
        memcpy(&in[ECAT_MOTW_OFF_COUNT], s_motShadow, region);
        return s_motPubCount;
    }

    /* ── 3. 重建密排区 ── */
    in[ECAT_MOTW_OFF_ACTIVE] = mt.active_count;
    in[ECAT_MOTW_OFF_BOOT]   = mt.boot_active;
    in[ECAT_MOTW_OFF_MAGIC]  = kEcatMotLayoutMagic;
    wr32le(&in[ECAT_MOTW_OFF_GLOBAL_SEQ], mt.global_seq);

    uint8_t  w = 0U;
    uint16_t newMask = 0U;
    for (uint8_t i = 0U; i < n; ++i) {
        const MotorSlot &s = *picks[i];
        if ((s.wr_seq & 1U) != 0U) {
            continue;   /* 写入中：本拍省略，下拍自愈 */
        }
        __DMB();

        uint8_t *p = &in[ECAT_MOTW_OFF_SLOTS + (uint16_t)w * kEcatMotSlotSize];
        p[ECAT_MOT_OFF_NODE]   = nodes[i];
        p[ECAT_MOT_OFF_CH]     = s.channel;
        p[ECAT_MOT_OFF_ONLINE] = s.online;
        p[ECAT_MOT_OFF_DLC]    = s.dlc;
        wr32le(&p[ECAT_MOT_OFF_SEQ], s.seq);
        memcpy(&p[ECAT_MOT_OFF_STATUS], s.data, kMotorStatusDlc);

        s_motPubSeq[nodes[i]]    = s.seq;
        s_motPubOnline[nodes[i]] = s.online;
        newMask |= (uint16_t)(1U << nodes[i]);
        ++w;
    }

    in[ECAT_MOTW_OFF_COUNT] = w;

    /* 更新影子：下次集合+seq 未变时整段复用 */
    memcpy(s_motShadow, &in[ECAT_MOTW_OFF_COUNT], motRegionLen(w));
    s_motPubMask   = newMask;
    s_motPubCount  = w;
    s_motShadowValid = true;
    return w;
}

bool EthercatCanBridge::peekOutput(const uint8_t *head)
{
    if (head == nullptr) {
        return false;
    }
    const uint8_t seq = head[ECAT_CAN_TX_OFF_SEQ];

    /* WIN@8 不再使用：1.6.21 电机整轮密排，无滑窗。 */

    if (!s_seqPrimed) {
        s_seqPrimed        = true;
        s_lastSeq          = seq;
        s_coldStartPending = true;
        return true; /* 冷启动首包强制全量读，避免漏掉首帧命令 */
    }

    if (seq != s_lastSeq) {
        return true;
    }
    return (head[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U;
}

void EthercatCanBridge::resetSequenceState()
{
    s_seqPrimed        = false;
    s_lastSeq          = 0U;
    s_coldStartPending = false;
}

void EthercatCanBridge::processOutput(const uint8_t *out)
{
    if (out == nullptr) {
        return;
    }

    const uint8_t seq = out[ECAT_CAN_TX_OFF_SEQ];

    if (!s_seqPrimed) {
        s_seqPrimed        = true;
        s_lastSeq          = seq;
        s_coldStartPending = true;
    }

    /* 无论 seq 是否变化，优先处理非 seq 驱动的命令（FLAGS bit0 清统计） */
    if ((out[ECAT_CAN_TX_OFF_FLAGS] & 0x01U) != 0U) {
        s_txCount     = 0U;
        s_dropCount   = 0U;
        s_rxCount     = 0U;
        s_rxPublished = true;
    }

    bool executeCommand = false;
    if (s_coldStartPending) {
        s_coldStartPending = false;
        /* 冷启动首包：若包含有效单帧命令或批量帧，立即执行 */
        const uint8_t dlc = out[ECAT_CAN_TX_OFF_DLC];
        const uint8_t batchCount = out[ECAT_TXB_OFF_COUNT];
        if (dlc > 0U || batchCount > 0U) {
            executeCommand = true;
        }
    } else if (seq != s_lastSeq) {
        executeCommand = true;
    }

    if (!executeCommand) {
        return;
    }
    s_lastSeq = seq;

    const uint8_t ch  = out[ECAT_CAN_TX_OFF_CH];
    uint8_t       dlc = out[ECAT_CAN_TX_OFF_DLC];
    if (dlc > kEcatCanTxDataMax) {
        dlc = kEcatCanTxDataMax;
    }

    if (dlc > 0U) {
        if (!CanFd::isChannelEnabled(ch)) {
            s_lastTxResult = (uint8_t)CanFd::CanTxResult::kErrInvalid;
        } else {
            const CanFd::CanTxResult r =
                CanFd::enqueueTx(ch, rd32le(&out[ECAT_CAN_TX_OFF_ID]),
                                 &out[ECAT_CAN_TX_OFF_DATA], dlc);
            s_lastTxResult = (uint8_t)r;
            if (r == CanFd::CanTxResult::kOk) {
                ++s_txCount;
            }
        }
    }

    (void)sendBatch(out);
}

void EthercatCanBridge::buildInput(uint8_t *in)
{
    if (in == nullptr) {
        return;
    }
    memset(in, 0, kEcatPdoInBytes);

    /* ── 段 1（0..31）：桥状态 + 短快照 ── */
    in[ECAT_CAN_RX_OFF_ACK]   = s_lastSeq;
    in[ECAT_CAN_RX_OFF_TXRES] = s_lastTxResult;
    wr16le(&in[ECAT_CAN_RX_OFF_TXCNT], s_txCount);
    wr16le(&in[ECAT_CAN_RX_OFF_DROP],  s_dropCount);
    wr32le(&in[ECAT_CAN_RX_OFF_RXCNT], s_rxCount);

    const bool xrxStable = ((s_xrxSeq & 1U) == 0U);

    if (xrxStable && s_rxValid) {
        uint8_t flags = 0x01U;
        if (s_rxTruncated) {
            flags |= 0x02U;
        }
        in[ECAT_CAN_RX_OFF_CH]    = s_rxCh;
        in[ECAT_CAN_RX_OFF_DLC]   = s_rxDlc;
        in[ECAT_CAN_RX_OFF_FLAGS] = flags;
        wr32le(&in[ECAT_CAN_RX_OFF_ID], s_rxId);
        memcpy(&in[ECAT_CAN_RX_OFF_DATA], s_rxData, kEcatCanRxDataMax);
        s_rxPublished = true;
    }

    const uint8_t ch = s_rxValid ? s_rxCh : 0U;
    const uint32_t backlog = CanFd::isChannelEnabled(ch) ? CanFd::txQueueBacklog(ch) : 0U;
    in[ECAT_CAN_RX_OFF_BACKLOG] = (backlog > 255U) ? 255U : (uint8_t)backlog;

    /* ── 段 2（32..543）：电机整轮密排（空槽不发，count=实际电机数）── */
    (void)fillMotorRound(in);

    /* ── 段 3（544..583）：最近一帧完整快照 ──
     * 与段 1 共用 xrxStable 门控，两段必须来自同一帧。 */
    if (xrxStable && s_rxValid) {
        in[ECAT_XRX_OFF_CH]    = s_rxCh;
        in[ECAT_XRX_OFF_DLC]   = s_rxDlc;
        in[ECAT_XRX_OFF_FLAGS] = 0x01U;
        wr32le(&in[ECAT_XRX_OFF_ID], s_rxId);
        memcpy(&in[ECAT_XRX_OFF_DATA], s_rxData, kEcatCanXRxDataMax);
    }

    /* 段 4（历史覆盖窗口）已删除；段 5 由 EthercatPdo::fillDiagTail 写。 */
}

void EthercatCanBridge::onCanFrame(uint8_t channel, uint32_t id,
                                   const uint8_t *data, uint8_t length)
{
    if (data == nullptr) {
        return;
    }

    uint8_t copyLen = length;
    if (copyLen > kEcatCanXRxDataMax) {
        copyLen = kEcatCanXRxDataMax;
    }

    /* ── 快照 seqlock：奇数=正在写，消费者本拍跳过段 1/段 3 ── */
    ++s_xrxSeq;
    __DMB();

    if (s_rxValid && !s_rxPublished) {
        ++s_dropCount;
    }

    memcpy(s_rxData, data, copyLen);
    if (copyLen < kEcatCanXRxDataMax) {
        memset(&s_rxData[copyLen], 0, (size_t)(kEcatCanXRxDataMax - copyLen));
    }

    s_rxCh        = channel;
    s_rxId        = id;
    s_rxDlc       = length;
    s_rxTruncated = (length > kEcatCanRxDataMax);
    ++s_rxCount;
    s_rxValid     = true;
    s_rxPublished = false;

    __DMB();
    ++s_xrxSeq;

    /* STATUS 走 MotorState → 段 2 整轮；非 STATUS 只更新段 1/3 最近一帧快照。
     * 1.6.21 起不再写入历史覆盖窗口。 */
}

extern "C" {

void EcatCan_ProcessOutput(const uint8_t *out)
{
    EthercatCanBridge::processOutput(out);
}

void EcatCan_BuildInput(uint8_t *in)
{
    EthercatCanBridge::buildInput(in);
}

void EcatCan_ResetSequenceState(void)
{
    EthercatCanBridge::resetSequenceState();
}

}  /* extern "C" */