/**
 * @file	   monitoring.c
 *
 * @copyright  Nehemis SARL reserves all rights even in the event of industrial
 *             property rights. We reserve all rights of disposal such as
 *             copying and passing on to third parties.
 *
 * @brief
 *
 */

/******************************************************************************
 * Include Header Files
 ******************************************************************************/
#include "enduranceTestBench.h"
#include "hal_system.h"
#include "stdlib.h"

/* CANopen includes */
#include <gen_define.h>
#include <co_canopen.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
/******************************************************************************
 * Type Declarations
 ******************************************************************************/
typedef enum
{
	TEST_BENCH_STARTUP,
    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_NODE_SKIPPED,
    TEST_BENCH_DATA_VERIF_SUCCESS,
    TEST_BENCH_DATA_VERIF_FAILURE,
} 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 uint8 currentNode_gu8 	= 0u;
static uint8 batchCompleted_u8 	= 1u;
static uint32 readPosition_gu32 = 0u;
static uint8 targetPositionStoredFlag_u8  = 0u;
static TestBenchData_en testBenchData_en;
static SdlTestBenchState_en testBenchState_en = TEST_BENCH_IDLE;

static uint64 readExecutedTime_u64;
static uint64 writeTime_u64;
/******************************************************************************
 * Public Function Definitions
 ******************************************************************************/
void EnduranceTestBenchRun(void)
{
    static uint64 startTime_u64 	= 0uLL;
    static uint8 alternate_u8 		= 0u;
    static uint8 retries_u8 		= 0u;
    uint64 currentTime_u64;

    HalSystemGetRunTimeMs(&currentTime_u64);

    if (startTime_u64 == 0uLL)
    {
        HalSystemGetRunTimeMs(&startTime_u64);
        testBenchState_en = TEST_BENCH_STARTUP;
    }

    switch (testBenchState_en)
    {
	case TEST_BENCH_STARTUP:
	{
		uint8 max_u8 = NMS_UINT8_MAX; /* Fully open (255) */
		uint8 min_u8 = 0u;            /* Fully closed (0) */

		if ((currentTime_u64 - startTime_u64) < 5000uLL)
		{
		    /* First 5 seconds: First 10 open, rest closed */
		    for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
		    {
		    	testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10u) ? max_u8 : min_u8;
		    }
		}
		else if ((currentTime_u64 - startTime_u64) < 10000uLL)
		{
		    /* Next 5 seconds: First 10 closed, rest open */
		    for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
		    {
		    	testBenchData_en.targetPositions_gau8[i_u8] = (i_u8 < 10) ? min_u8 : max_u8;
		    }
		}
		else
		{
		    /* After 10 seconds, move to endurance test */
		    batchCompleted_u8 	= 1u;
		    testBenchState_en 	= TEST_BENCH_IDLE;
		}

		currentNode_gu8 		= 0u;
		testBenchState_en 		= TEST_BENCH_BATCH_WRITE;
	}
	break;

	case TEST_BENCH_IDLE:
	{
	    if (batchCompleted_u8)
	    {
	        batchCompleted_u8 = 0u;  					/* Lock batch update until all nodes confirm their positions */

	        uint8 max_u8 = NMS_UINT8_MAX;        		/* Fully open (255) */
	        uint8 min_u8 = 0u;                   		/* Fully closed (0) */
	        uint8 currentGroup_u8 = alternate_u8 % 2;  	/* Alternates between two patterns */

	        /* Check if it's a random cycle */
	        if ((alternate_u8 % 2) == 1)
	        {
	            /* Randomized cycle */
	            for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
	            {
	            	testBenchData_en.targetPositions_gau8[i_u8] = (uint8)(rand() % 256);  /* Assign random value between 0-255 */
	            }
	        }
	        else
	        {
	            /* Normal alternating open-close cycle */
	            for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
	            {
	                if (((i_u8 / 5) % 2) == currentGroup_u8)
	                {
	                	testBenchData_en.targetPositions_gau8[i_u8] = max_u8;  /* Fully open */
	                }
	                else
	                {
	                	testBenchData_en.targetPositions_gau8[i_u8] = min_u8;  /* Fully closed */
	                }
	            }
	        }

	        alternate_u8++;  /* Switch to the next cycle pattern */
	        currentNode_gu8 = 0u;
	        testBenchState_en = TEST_BENCH_BATCH_WRITE;
	    }
	}
	break;

	/* -------------------- Batch Write -------------------- */
    case TEST_BENCH_BATCH_WRITE:
    {
        if (currentNode_gu8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT)
        {
            if (targetPositionStoredFlag_u8  == 0u)
            {
                uint8 value_u8 = (uint8)(rand() % 256);

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

                if (retVal_en == RET_OK)
                {
                	testBenchData_en.targetPositions_gau8[currentNode_gu8] = value_u8;
                    targetPositionStoredFlag_u8  = 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_NODE_SKIPPED;

                        /* Mark node as skipped and move on */
                        currentNode_gu8++;
                        retries_u8 = 0u;
                        targetPositionStoredFlag_u8  = 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++;
            targetPositionStoredFlag_u8  = 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_NODE_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_NODE_SKIPPED)
            {
                if (abs(testBenchData_en.targetPositions_gau8[currentNode_gu8] -
                        testBenchData_en.readPosition_gau8[currentNode_gu8]) <= 5)     /* Accepted difference between read and write for successful operation */
                {
                    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, can log or process the batch results here */
        /* Reset all node data for the next cycle */
        for (uint8 i_u8 = 0u; i_u8 < ENDURANCE_TEST_BENCH_LSS_NODE_COUNT; i_u8++)
        {
            testBenchData_en.targetPositions_gau8[i_u8] = 0u;
            testBenchData_en.readPosition_gau8[i_u8]    = 0u;
            testBenchData_en.status_en[i_u8]            = TEST_BENCH_DATA_VERIF_DEFAULT;
        }

        /* Reset everything and Start next cycle */
        currentNode_gu8 	= 0u;
        targetPositionStoredFlag_u8  	= 0u;
        retries_u8			= 0u;
        testBenchState_en 	= TEST_BENCH_BATCH_WRITE;
        break;
    }

    default:
        break;
}
}

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

void EnduranceTestBenchReadInd(uint8 sdoNr_u8, uint16 index_u16, uint8 subIndex_u8, uint32 size, uint32 errorVal_u32)
{
	HalSystemGetRunTimeMs(&readExecutedTime_u64);
}