#include "plsr_self_test.h"
#include "plc_device.h"
#include "plsr_core.h"
#include "plsr_job.h"
#include <string.h>

/* 上电自测（验证后可删除）：
 * - AB 模式使用 Q0(A)/Q1(B)，其余用出厂默认参数（K1）
 * - 任务：3 段完整 AB 周期（H00 完成，顺序衔接）：
 *   段1：2000Hz / +1000 周期（A 超前 B）
 *   段2：5000Hz / +6000 周期（A 超前 B）
 *   段3：1000Hz / -500 周期（B 超前 A，验证反向）
 * 数据源为静态数组，仅自测使用（正式 D 设备适配器见 Modbus 阶段）。 */

#define SELF_TEST_WORD_CAPACITY     (192U)
#define SELF_TEST_S0_BASE           (10U)
#define SELF_TEST_S1_BASE           (60U)
#define SELF_TEST_DIR_POINT         (4U)
#define SELF_TEST_SFD_AXIS_STRIDE   (130U)
#define SELF_TEST_SFD_SET_OFFSET    (50U)

static uint16_t SelfTestWords[3][SELF_TEST_WORD_CAPACITY];

static uint8_t SelfTestValidateWords(void *context,
                                     PLSR_DEVICE_TYPE device,
                                     uint32_t firstAddress,
                                     uint32_t wordCount)
{
    (void)context;
    if ((device > PLSR_DEVICE_FD) || (wordCount == 0UL))
    {
        return 0U;
    }
    return (((uint64_t)firstAddress + wordCount)
            <= SELF_TEST_WORD_CAPACITY)
               ? 1U
               : 0U;
}

static uint8_t SelfTestReadWord(void *context,
                                PLSR_DEVICE_TYPE device,
                                uint32_t address,
                                uint16_t *value)
{
    (void)context;
    if ((device > PLSR_DEVICE_FD) || (value == NULL)
        || (address >= SELF_TEST_WORD_CAPACITY))
    {
        return 0U;
    }
    *value = SelfTestWords[device][address];
    return 1U;
}

static uint8_t SelfTestReadBit(void *context,
                               PLSR_DEVICE_TYPE device,
                               uint32_t address,
                               uint8_t *value)
{
    (void)context;
    (void)device;
    (void)address;
    *value = 0U;
    return 1U;
}

static void SelfTestWriteDword(PLSR_DEVICE_TYPE device,
                               uint32_t address,
                               uint32_t value)
{
    SelfTestWords[device][address] = (uint16_t)(value & 0xFFFFUL);
    SelfTestWords[device][address + 1UL] = (uint16_t)(value >> 16U);
}

static void SelfTestWriteSfdDword(uint16_t address, uint32_t value)
{
    (void)PlcDeviceWriteSfd(address, (uint16_t)(value & 0xFFFFUL));
    (void)PlcDeviceWriteSfd((uint16_t)(address + 1U),
                            (uint16_t)(value >> 16U));
}

PLSR_RESULT PlsrSelfTestQueue(void)
{
    PLSR_CALL call;

    (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

    /* 方向端子 Y4（PULSE/DIR 模式必需，接线参数）。 */
    (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);
    (void)PlcDeviceWriteSfd(912U, 0U);
    (void)PlcDeviceWriteSfd(915U, 0xFFFFU);

    /* S0：3 段，H00 完成，顺序跳转。 */
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 3U);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 2000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 1000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 5000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, 6000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 30U, 1000UL);
    SelfTestWriteDword(PLSR_DEVICE_D,
                       SELF_TEST_S0_BASE + 32U,
                       (uint32_t)(int32_t)-500);

    /* S1：相对模式，起始段 0（=段1）。 */
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

    (void)memset(&call, 0, sizeof(call));
    call.sequence = 0xA5A5UL;
    call.source.context = NULL;
    call.source.validateWords = SelfTestValidateWords;
    call.source.readWord = SelfTestReadWord;
    call.source.readBit = SelfTestReadBit;
    call.s0.device = PLSR_DEVICE_D;
    call.s0.address = SELF_TEST_S0_BASE;
    call.s1.device = PLSR_DEVICE_D;
    call.s1.address = SELF_TEST_S1_BASE;
    call.s2.type = PLSR_OPERAND_CONSTANT;
    call.s2.constant = 1;
    call.dAxis = 0U;
    call.outputModeOverride = PLSR_OUTPUT_AB;
    return PlsrPostCall(&call);
}

PLSR_RESULT PlsrEquivalentSelfTestQueue(void)
{
    PLSR_CALL call;

    (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

    /* SFD900 Bit10~8=001（1um当量）；3脉冲/2单位。 */
    (void)PlcDeviceWriteSfd(900U, (1U << 8U));
    SelfTestWriteSfdDword(902U, 3UL);
    SelfTestWriteSfdDword(904U, 2UL);
    (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);
    (void)PlcDeviceWriteSfd(907U, 10U);
    (void)PlcDeviceWriteSfd(912U, 0U);
    (void)PlcDeviceWriteSfd(915U, 0xFFFFU);
    /* 当量换算后物理最高速度=90000Hz，不超过硬件100kHz。 */
    SelfTestWriteSfdDword(956U, 60000UL);

    /* 板端可观察版本：两段各1001工程单位。
     * 3脉冲/2单位带余数换算后分别输出1501、1502脉冲，累计3003脉冲；
     * 1500Hz附近持续约2s，避免原3脉冲自检在逻辑分析仪启动前已经结束。 */
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 2U);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 10U, 1000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 12U, 1001UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 20U, 1000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE + 22U, 1001UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

    (void)memset(&call, 0, sizeof(call));
    call.sequence = 0xA5A6UL;
    call.source.context = NULL;
    call.source.validateWords = SelfTestValidateWords;
    call.source.readWord = SelfTestReadWord;
    call.source.readBit = SelfTestReadBit;
    call.s0.device = PLSR_DEVICE_D;
    call.s0.address = SELF_TEST_S0_BASE;
    call.s1.device = PLSR_DEVICE_D;
    call.s1.address = SELF_TEST_S1_BASE;
    call.s2.type = PLSR_OPERAND_CONSTANT;
    call.s2.constant = 1;
    call.dAxis = 0U;
    call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;
    return PlsrPostCall(&call);
}

PLSR_RESULT PlsrProtectionSelfTestQueue(void)
{
    PLSR_CALL call;
    PLSR_COMMAND command;
    PLSR_RESULT result;

    (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

    /* Pulse unit, soft limits enabled, 1 pulse per position unit. */
    (void)PlcDeviceWriteSfd(900U, (1U << 2U));
    SelfTestWriteSfdDword(902U, 1UL);
    SelfTestWriteSfdDword(904U, 1UL);
    (void)PlcDeviceWriteSfd(906U, SELF_TEST_DIR_POINT);
    (void)PlcDeviceWriteSfd(907U, 10U);
    (void)PlcDeviceWriteSfd(912U, 0U);
    (void)PlcDeviceWriteSfd(915U, 0xFFFFU);
    SelfTestWriteSfdDword(930U, 500UL);
    SelfTestWriteSfdDword(932U, (uint32_t)(int32_t)-500);

    /* K1: 1000Hz, 100ms acceleration/deceleration, 1ms refresh. */
    SelfTestWriteSfdDword(950U, 1000UL);
    (void)PlcDeviceWriteSfd(952U, 100U);
    (void)PlcDeviceWriteSfd(953U, 100U);
    (void)PlcDeviceWriteSfd(954U, 0U);
    (void)PlcDeviceWriteSfd(955U, 0U);
    SelfTestWriteSfdDword(956U, 100000UL);
    SelfTestWriteSfdDword(958U, 1000UL);
    SelfTestWriteSfdDword(960U, 0UL);
    (void)PlcDeviceWriteSfd(962U, 50U);
    (void)PlcDeviceWriteSfd(963U, 0U);
    (void)PlcDeviceWriteSfd(964U, 0U);
    SelfTestWriteSfdDword(966U, 2000UL);
    SelfTestWriteSfdDword(968U, 200UL);

    /* One relative segment requests +10000 pulses; +500 must stop it. */
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S0_BASE, 1U);
    SelfTestWriteDword(PLSR_DEVICE_D,
                       SELF_TEST_S0_BASE + 10U,
                       1000UL);
    SelfTestWriteDword(PLSR_DEVICE_D,
                       SELF_TEST_S0_BASE + 12U,
                       10000UL);
    SelfTestWriteDword(PLSR_DEVICE_D, SELF_TEST_S1_BASE, 0U);

    (void)memset(&command, 0, sizeof(command));
    command.sequence = 0xA5A7UL;
    command.axis = 0U;
    command.opcode = PLSR_CMD_SET_POSITION;
    command.argument = 0;
    result = PlsrPostCommand(&command);
    if (result != PLSR_RESULT_QUEUED)
    {
        return result;
    }

    (void)memset(&call, 0, sizeof(call));
    call.sequence = 0xA5A8UL;
    call.source.context = NULL;
    call.source.validateWords = SelfTestValidateWords;
    call.source.readWord = SelfTestReadWord;
    call.source.readBit = SelfTestReadBit;
    call.s0.device = PLSR_DEVICE_D;
    call.s0.address = SELF_TEST_S0_BASE;
    call.s1.device = PLSR_DEVICE_D;
    call.s1.address = SELF_TEST_S1_BASE;
    call.s2.type = PLSR_OPERAND_CONSTANT;
    call.s2.constant = 1;
    call.dAxis = 0U;
    call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;
    return PlsrPostCall(&call);
}

PLSR_RESULT PlsrFourAxisSelfTestQueue(void)
{
    static const uint16_t s0Base[PLSR_AXIS_COUNT] =
    {
        10U, 40U, 70U, 100U
    };
    static const uint16_t s1Base[PLSR_AXIS_COUNT] =
    {
        160U, 164U, 168U, 172U
    };
    static const uint32_t frequencyHz[PLSR_AXIS_COUNT] =
    {
        1000UL, 2000UL, 3000UL, 4000UL
    };
    static const int32_t pulseCount[PLSR_AXIS_COUNT] =
    {
        1000, 2000, 3000, 4000
    };
    PLSR_CALL call;
    PLSR_COMMAND command;
    PLSR_RESULT result;
    uint16_t commonBase;
    uint16_t setBase;
    uint8_t axis;

    (void)memset(SelfTestWords, 0, sizeof(SelfTestWords));

    for (axis = 0U; axis < PLSR_AXIS_COUNT; axis++)
    {
        commonBase = (uint16_t)(900U
                                + (uint16_t)axis
                                      * SELF_TEST_SFD_AXIS_STRIDE);
        setBase = (uint16_t)(commonBase + SELF_TEST_SFD_SET_OFFSET);

        /* Pulse unit, PULSE/DIR, no limit input, Q4..Q7 as DIR. */
        (void)PlcDeviceWriteSfd(commonBase, 0U);
        SelfTestWriteSfdDword((uint16_t)(commonBase + 2U), 1UL);
        SelfTestWriteSfdDword((uint16_t)(commonBase + 4U), 1UL);
        (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 6U),
                                (uint16_t)(SELF_TEST_DIR_POINT + axis));
        (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 7U), 10U);
        (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 12U), 0U);
        (void)PlcDeviceWriteSfd((uint16_t)(commonBase + 15U), 0xFFFFU);

        /* K1 has no ramp so all four channels keep an exact fixed rate. */
        SelfTestWriteSfdDword(setBase, frequencyHz[axis]);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 2U), 0U);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 3U), 0U);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 4U), 0U);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 5U), 0U);
        SelfTestWriteSfdDword((uint16_t)(setBase + 6U), 100000UL);
        SelfTestWriteSfdDword((uint16_t)(setBase + 8U),
                              frequencyHz[axis]);
        SelfTestWriteSfdDword((uint16_t)(setBase + 10U), 0UL);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 12U), 50U);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 13U), 0U);
        (void)PlcDeviceWriteSfd((uint16_t)(setBase + 14U), 0U);
        SelfTestWriteSfdDword((uint16_t)(setBase + 16U), 2000UL);
        SelfTestWriteSfdDword((uint16_t)(setBase + 18U), 200UL);

        SelfTestWriteDword(PLSR_DEVICE_D, s0Base[axis], 1U);
        SelfTestWriteDword(PLSR_DEVICE_D,
                           (uint32_t)s0Base[axis] + 10UL,
                           frequencyHz[axis]);
        SelfTestWriteDword(PLSR_DEVICE_D,
                           (uint32_t)s0Base[axis] + 12UL,
                           (uint32_t)pulseCount[axis]);
        SelfTestWriteDword(PLSR_DEVICE_D, s1Base[axis], 0U);

        /* Make the board-test result independent of a previously restored
         * Backup SRAM position. */
        (void)memset(&command, 0, sizeof(command));
        command.sequence = 0xA500UL + axis;
        command.axis = axis;
        command.opcode = PLSR_CMD_SET_POSITION;
        command.argument = 0;
        result = PlsrPostCommand(&command);
        if (result != PLSR_RESULT_QUEUED)
        {
            return result;
        }

        (void)memset(&call, 0, sizeof(call));
        call.sequence = 0xA600UL + axis;
        call.source.context = NULL;
        call.source.validateWords = SelfTestValidateWords;
        call.source.readWord = SelfTestReadWord;
        call.source.readBit = SelfTestReadBit;
        call.s0.device = PLSR_DEVICE_D;
        call.s0.address = s0Base[axis];
        call.s1.device = PLSR_DEVICE_D;
        call.s1.address = s1Base[axis];
        call.s2.type = PLSR_OPERAND_CONSTANT;
        call.s2.constant = 1;
        call.dAxis = axis;
        call.outputModeOverride = PLSR_OUTPUT_PULSE_DIR;
        result = PlsrPostCall(&call);
        if (result != PLSR_RESULT_QUEUED)
        {
            return result;
        }
    }
    return PLSR_RESULT_QUEUED;
}