/**
 * @file ethercat_slave.cpp
 * @brief EtherCAT 从站周期处理实现。
 */
#include "comm/ethercat_slave.h"
#include "comm/esc_lan9252.h"
#include "sys/config.h"
#include "sys/task_manager.h"
#include "main.h"

PdoBuffer EthercatSlave::s_inputs[3] = {};
PdoBuffer EthercatSlave::s_outputs[3] = {};
PdoBuffer EthercatSlave::s_appInputs = {};
PdoBuffer EthercatSlave::s_appOutputs = {};
volatile uint8_t EthercatSlave::s_inputWriteIdx = 0U;
volatile uint8_t EthercatSlave::s_outputReadIdx = 0U;
volatile uint16_t EthercatSlave::s_periodicSeq = 0U;
uint16_t EthercatSlave::s_processedSeq = 0U;
volatile uint32_t EthercatSlave::s_isrMaxCycles = 0U;
volatile uint32_t EthercatSlave::s_isrAvgCycles = 0U;
volatile uint32_t EthercatSlave::s_isrCount = 0U;
bool EthercatSlave::s_escReady = false;
uint32_t EthercatSlave::s_chipId = 0U;
uint8_t EthercatSlave::s_alState = 0U;

/* ISR 执行时间评估:
 * - 若 DWT 实测 max > CONFIG_ETHERCAT_ISR_MAX_US_THRESHOLD(5µs),需评估:
 *   a) 减小 CONFIG_ETHERCAT_PDO_SIZE(默认 32 字节)
 *   b) 优化 EXMC 时序参数(esc_exmc_bus.cpp 中 read_latency/write_latency)
 * - 若 max ≤ 5µs,接受现状;preempt=0 阻塞窗口 ≤ 5µs,CAN/SPI 中断需容忍此窗口。
 *   SPI0 从机在此窗口内不会 RX 溢出(假设 SPI 时钟 ≤ 10MHz,5µs ≥ 50 bit,远小于一帧)。 */

void EthercatSlave::init()
{
    EscLan9252::init();

    /* 第 1 层调试可观测性:读取芯片 ID 并打印 selfTest 结果 */
    s_chipId = EscLan9252::readChipId();
    const bool chipIdOk = (s_chipId == 0x87654321U);

    /* AL 状态守卫:仅在芯片 ID 匹配时读取 AL 状态(ID 不匹配时 EXMC 可能读回 0/0xFFFF,无意义)。
     * AL ≥ SAFEOP(0x04) 才认为 SYNC0 可触发;完整 ESC 配置(SM/FMMU/AL Control/SYNC0)需 SSC 集成。 */
    s_alState = 0U;
    if (chipIdOk) {
        s_alState = (uint8_t)(EscLan9252::initOnlyReadReg(0x0130U) & 0x000FU);
    }
    s_escReady = chipIdOk && (s_alState >= AL_STATE_SAFEOP);

    if (!chipIdOk) {
        printf("[ECAT] ESC chip id=0x%08lX selfTest=FAIL\r\n", (unsigned long)s_chipId);
        printf("[ECAT] ESC NOT ready, sync ISR disabled (check EXMC wiring/timing)\r\n");
    } else if (s_alState < AL_STATE_SAFEOP) {
        printf("[ECAT] ESC chip id=0x%08lX selfTest=PASS, but AL state=0x%02X < SAFEOP(0x%02X)\r\n",
               (unsigned long)s_chipId, (unsigned)s_alState, (unsigned)AL_STATE_SAFEOP);
        printf("[ECAT] SYNC0 will not fire, full ESC config requires SSC integration\r\n");
    } else {
        printf("[ECAT] ESC ready, chip id=0x%08lX, AL state=0x%02X\r\n",
               (unsigned long)s_chipId, (unsigned)s_alState);
    }

    s_inputWriteIdx = 0U;
    s_outputReadIdx = 0U;
    s_periodicSeq = 0U;
    s_processedSeq = 0U;
}

__attribute__((section(".ramfunc"), noinline))
void EthercatSlave::periodicIsr()
{
    const uint32_t cycStart = DWT->CYCCNT;

    /* 1. 读取输入 PDO 到当前写入缓冲 */
    EscLan9252::readPdi(CONFIG_ESC_PDI_INPUT_OFFSET, s_inputs[s_inputWriteIdx].data, CONFIG_ETHERCAT_PDO_SIZE);
    s_inputs[s_inputWriteIdx].len = CONFIG_ETHERCAT_PDO_SIZE;
    s_inputWriteIdx = (s_inputWriteIdx + 1U) % 3U;

    /* 2. 写入输出 PDO(从当前读取缓冲) */
    EscLan9252::writePdi(CONFIG_ESC_PDI_OUTPUT_OFFSET, s_outputs[s_outputReadIdx].data, s_outputs[s_outputReadIdx].len);
    s_outputReadIdx = (s_outputReadIdx + 1U) % 3U;

    /* 3. 递增周期序列号通知主循环(替代 TaskManager::set,避免 BASEPRI 竞态) */
    s_periodicSeq++;

    const uint32_t elapsed = DWT->CYCCNT - cycStart;
    s_isrCount++;
    if (elapsed > s_isrMaxCycles) {
        s_isrMaxCycles = elapsed;
    }
    /* 滑动平均:avg = avg + (elapsed - avg) / 16 (近似 EMA,避免除法精度问题) */
    s_isrAvgCycles = s_isrAvgCycles + ((int32_t)elapsed - (int32_t)s_isrAvgCycles) / 16;
}

const PdoBuffer* EthercatSlave::getLatestInputs()
{
    /* 返回最新完成的缓冲 = (writeIdx + 2) % 3,与 ISR 当前写入位置隔一个 */
    return &s_inputs[(s_inputWriteIdx + 2U) % 3U];
}

void EthercatSlave::setOutputs(const PdoBuffer *outputs)
{
    if (outputs == nullptr) {
        return;
    }
    /* 写入与 ISR 当前读取位置隔 2 个缓冲,提供 2 周期宽限。
     * 2 周期宽限下拷贝竞态窗口消除,前提是主循环处理时间 < 2 × 周期时间。 */
    const uint8_t write_idx = (s_outputReadIdx + 2U) % 3U;
    s_outputs[write_idx] = *outputs;
}

void EthercatSlave::process()
{
    /* 应用层 PDO 桥接(非实时,由主循环周期序列号或 ESC IRQ 触发)。
     * 运行期 PDO 交换仅通过 getLatestInputs()/setOutputs() 三缓冲进行;ESC 寄存器访问仅限 init 阶段。
     *
     * 框架级桥接:从三缓冲取最新输入到 s_appInputs,把 s_appOutputs 推回三缓冲。
     * TODO: 应用对象字典映射(具体输入字节对应哪个应用变量,如字节 0-3=温度浮点),
     *       由后续应用层 spec 定义;应用层通过 getApplicationInputs()/setApplicationOutputs() 访问快照。 */
    const PdoBuffer *latest = getLatestInputs();
    if (latest != nullptr) {
        s_appInputs = *latest;
    }
    setOutputs(&s_appOutputs);
}

const PdoBuffer* EthercatSlave::getApplicationInputs()
{
    return &s_appInputs;
}

void EthercatSlave::setApplicationOutputs(const PdoBuffer *outputs)
{
    if (outputs == nullptr) {
        return;
    }
    s_appOutputs = *outputs;
}

bool EthercatSlave::hasPendingPeriodic()
{
    return s_periodicSeq != s_processedSeq;
}

void EthercatSlave::markPeriodicProcessed()
{
    s_processedSeq = s_periodicSeq;
}

void EthercatSlave::getIsrStats(uint32_t *maxCycles, uint32_t *avgCycles, uint32_t *count)
{
    if (maxCycles) *maxCycles = s_isrMaxCycles;
    if (avgCycles) *avgCycles = s_isrAvgCycles;
    if (count)     *count     = s_isrCount;
}