/**
 * @file       batch_endurance_testbench.c
 *
 * @brief      Batch mode test bench:
 *             1. Write to all nodes (at 100ms intervals)
 *             2. Read all nodes
 *             3. Verify positions (retry mismatch up to 2 times)
 *             4. Cycle complete – reset for next iteration
 *
 * @copyright
 */

#include "enduranceTestBench.h"
#include "stdlib.h"

/* CANopen includes */
#include <gen_define.h>
#include <co_canopen.h>
#include "../nms_hal/hal_system.h"

/******************************************************************************
 * Macro constant declarations
 ******************************************************************************/
#define ENDURANCE_TEST_BENCH_MAX_RETRY_CNT              3u
#define ENDURANCE_TEST_BENCH_TIMEOUT                    1000u
#define ENDURANCE_TEST_BENCH_LSS_NODE_COUNT             20u
#define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX     0x6002
#define ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX 0x0
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX     0x6004
#define ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX 0x0

/* Batch delay between node operations (in ms) */
#define NODE_OP_DELAY_MS                                100u

/******************************************************************************
 * Type Declarations
 ******************************************************************************/
typedef enum
{
    TEST_BENCH_BATCH_WRITE,          /* Write to nodes in batch */
    TEST_BENCH_BATCH_WRITE_WAIT,     /* Wait between writes */
    TEST_BENCH_BATCH_READ,           /* Read all nodes */
    TEST_BENCH_BATCH_READ_WAIT,      /* Wait between reads */
    TEST_BENCH_BATCH_VERIFY,         /* Verify all nodes */
    TEST_BENCH_CYCLE_COMPLETE,       /* End of cycle, reset for next iteration */
    TEST_BENCH_IDLE                  /* Idle state (optional) */
} SdlTestBenchState_en;

typedef enum
{
    TEST_BENCH_DATA_VERIF_DEFAULT,
    TEST_BENCH_DATA_VERIF_SUCCESS,
    TEST_BENCH_DATA_VERIF_FAILURE,
    TEST_BENCH_DATA_VERIF_SKIPPED,
} TestBenchStatus_en;

typedef struct
{
    uint8 targetPositions_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
    uint8 readPosition_gau8[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
    TestBenchStatus_en status_en[ENDURANCE_TEST_BENCH_LSS_NODE_COUNT];
} TestBenchData_en;

/******************************************************************************
 * Global variable declarations
 ******************************************************************************/
static TestBenchData_en testBenchData_en;
static uint8 currentNode_gu8           = 0u;
static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE;
static uint32 readPosition_gu32;
static uint8 storedTargetPos           = 0u;   /* Flag: target written to current node */
static uint8 readMismatchRetries_u8    = 0u;   /* Retry count for verify mismatches */

static uint64 writeTime_u64            = 0uLL;
static uint64 readExecutedTime_u64     = 0uLL;
static uint64 cycleStartTime_u64       = 0uLL;
static uint8 retries_u8                = 0u;   /* General retry counter for SDO communication */

/******************************************************************************
 * Public Function Definitions
 ******************************************************************************/
void TestRun(void)
{
    uint64 currentTime_u64;
    HalSystemGetRunTimeMs(&currentTime_u64);

    /* Initialization: start new cycle if in idle or cycle complete state */
    if (testBenchState_en == TEST_BENCH_IDLE || testBenchState_en == TEST_BENCH_CYCLE_COMPLETE)
    {
        /* Prepare new cycle: reset all node statuses and choose new target positions */
        for (uint8 i = 0u; i < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i++)
        {
            testBenchData_en.targetPositions_gau8[i] = (uint8)(rand() % 256);
            testBenchData_en.readPosition_gau8[i] = 0u;
            testBenchData_en.status_en[i] = TEST_BENCH_DATA_VERIF_DEFAULT;
        }
        currentNode_gu8 = 0u;
        storedTargetPos = 0u;
        retries_u8 = 0u;
        readMismatchRetries_u8 = 0u;
        cycleStartTime_u64 = currentTime_u64;
        testBenchState_en = TEST_BENCH_BATCH_WRITE;
    }

    switch (testBenchState_en)
    {
        /* -------------------- Batch Write -------------------- */
        case TEST_BENCH_BATCH_WRITE:
        {
            if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
            {
                if (storedTargetPos == 0)
                {
                    /* Use the pre-assigned target value for this node */
                    uint8 value_u8 = testBenchData_en.targetPositions_gau8[currentNode_gu8];

                    RET_T retVal_en = coSdoWrite(
                        (currentNode_gu8 + 1),
                        ENDURANCE_TEST_BENCH_POSITION_SETPOINT_INDEX,
                        ENDURANCE_TEST_BENCH_POSITION_SETPOINT_SUB_INDEX,
                        &value_u8,
                        sizeof(uint8),
                        CO_FALSE,
                        ENDURANCE_TEST_BENCH_TIMEOUT
                    );

                    if (retVal_en == RET_OK)
                    {
                        storedTargetPos = 1; /* Mark that target is written for this node */
                        HalSystemGetRunTimeMs(&writeTime_u64);
                        retries_u8 = 0u;
                        testBenchState_en = TEST_BENCH_BATCH_WRITE_WAIT;
                    }
                    else
                    {
                        retries_u8++;
                        if (retries_u8 >= ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
                        {
                            testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_SKIPPED;
                            /* Mark node as skipped and move on */
                            currentNode_gu8++;
                            retries_u8 = 0u;
                            storedTargetPos = 0u;
                            testBenchState_en = TEST_BENCH_BATCH_WRITE;
                        }
                    }
                }
            }
            else
            {
                /* Finished writing to all nodes */
                currentNode_gu8 = 0u;   /* Reset index for read phase */
                testBenchState_en = TEST_BENCH_BATCH_READ;
            }
            break;
        }

        case TEST_BENCH_BATCH_WRITE_WAIT:
        {
            /* Wait 100ms between writes */
            if ((currentTime_u64 - writeTime_u64) >= 500)
            {
                /* Move to next node write */
                currentNode_gu8++;
                storedTargetPos = 0u;  /* Reset for next node */
                testBenchState_en = TEST_BENCH_BATCH_WRITE;
            }
            break;
        }

        /* -------------------- Batch Read -------------------- */
        case TEST_BENCH_BATCH_READ:
        {
            if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
            {
                RET_T retVal_en = coSdoRead(
                    (currentNode_gu8 + 1),
                    ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_INDEX,
                    ENDURANCE_TEST_BENCH_POSITION_FEEDBACK_SUB_INDEX,
                    (uint8*)&readPosition_gu32,
                    sizeof(uint32),
                    CO_FALSE,
                    ENDURANCE_TEST_BENCH_TIMEOUT
                );

                if (retVal_en == RET_OK)
                {
                    retries_u8 = 0u;
                    /* The read indication callback is assumed to update
                     * testBenchData_en.readPosition_gau8[currentNode_gu8] and
                     * then we simply continue to next node after a small delay.
                     */
                    HalSystemGetRunTimeMs(&readExecutedTime_u64);
                    testBenchData_en.readPosition_gau8[currentNode_gu8] = (uint8)readPosition_gu32;
                    testBenchState_en = TEST_BENCH_BATCH_READ_WAIT;
                }
                else if (retVal_en == RET_SERVICE_BUSY && retries_u8 < ENDURANCE_TEST_BENCH_MAX_RETRY_CNT)
                {
                    retries_u8++;
                }
                else
                {
                    /* If read fails, mark as skipped and move on */
                    testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_SKIPPED;
                    currentNode_gu8++;
                    retries_u8 = 0u;
                }
            }
            else
            {
                /* Finished reading all nodes */
                currentNode_gu8 = 0u;  /* Reset for verification phase */
                testBenchState_en = TEST_BENCH_BATCH_VERIFY;
            }
            break;
        }

        case TEST_BENCH_BATCH_READ_WAIT:
        {
            /* Wait 100ms between node reads */
            if ((currentTime_u64 - readExecutedTime_u64) >= 10)
            {
                currentNode_gu8++;
                testBenchState_en = TEST_BENCH_BATCH_READ;
            }
            break;
        }

        /* -------------------- Batch Verify -------------------- */
        case TEST_BENCH_BATCH_VERIFY:
        {
            if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
            {
                /* Process only if the node was not already skipped */
                if (testBenchData_en.status_en[currentNode_gu8] != TEST_BENCH_DATA_VERIF_SKIPPED)
                {
                    uint8 diff = abs(testBenchData_en.targetPositions_gau8[currentNode_gu8] -
                                     testBenchData_en.readPosition_gau8[currentNode_gu8]);

                    if (diff <= 1)
                    {
                        testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_SUCCESS;
                    }
                    else
                    {
                        testBenchData_en.status_en[currentNode_gu8] = TEST_BENCH_DATA_VERIF_FAILURE;
                    }
                }
                /* Move to the next node in any case */
                currentNode_gu8++;
                testBenchState_en = TEST_BENCH_BATCH_VERIFY;
            }
            else
            {
                /* All nodes verified; complete cycle */
                testBenchState_en = TEST_BENCH_CYCLE_COMPLETE;
            }
            break;
        }

        /* -------------------- Cycle Complete -------------------- */
        case TEST_BENCH_CYCLE_COMPLETE:
        {
            /* Optionally, you can log or process the batch results here */
            /* Reset all node data for the next cycle */
            for (uint8 i = 0u; i < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i++)
            {
                testBenchData_en.targetPositions_gau8[i] = 0u;
                testBenchData_en.readPosition_gau8[i]    = 0u;
                testBenchData_en.status_en[i]            = TEST_BENCH_DATA_VERIF_DEFAULT;
            }
            currentNode_gu8 = 0u;
            storedTargetPos = 0u;
            retries_u8 = 0u;
            readMismatchRetries_u8 = 0u;
            /* Start next cycle */
            testBenchState_en = TEST_BENCH_IDLE;
            break;
        }

        default:
            break;
    }
}

/******************************************************************************
 * Callback Functions
 ******************************************************************************/
void TestReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 errorVal_u32)
{
    HalSystemGetRunTimeMs(&writeTime_u64);
}

void TestWriteInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32)
{
    HalSystemGetRunTimeMs(&readExecutedTime_u64);
    //testBenchData_en.readPosition_gau8[currentNode_gu8] = (uint8)readPosition_gu32;
}